💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

🎁完整资源、论文复现、期刊合作、论文辅导及科研仿真定制事宜点击：

👉👉👉本文完整资源下载

⛳️座右铭：行百里者，半于九十。

⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能解答你胸中升起的一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥第一部分——内容介绍

摘要

面向高速工况下智能车辆的轨迹跟踪需求，本文依托 CarSim/Simulink 联合仿真平台，构建了一套以二自由度横向动力学为被控对象、以线性时变模型预测控制（LTV-MPC）为核心的横向控制系统，并同步实现了纯跟踪、Stanley 与加权误差反馈三种对照控制器，形成统一平台下的横向控制基准对比体系。参考路径由余弦型曲线经三次样条平滑生成，其航向角与曲率均由解析求导直接给出，避免了数值差分引入的噪声。控制器设计沿"非线性模型—工作点线性化—前向欧拉离散—增广增量建模—一步非线性残差补偿—预测方程装配—二次规划求解"的完整链条展开，将跟踪任务转化为同时含转角幅值与转角速率硬约束的标准凸二次规划问题，并借助松弛变量与求解失败兜底逻辑保证约束冲突与极端工况下的可行性。工程实现层面，本文系统讨论了信号单位换算、全局控制记忆与代数环规避、求解器容差设置、计算复杂度评估以及各权重参数的整定规律。仿真结果表明，该 MPC 方案在 90 km/h 车速下能够对中等曲率弯曲路径实现平稳、精确的跟踪，横向误差与航向误差的实时曲线为控制器性能评价提供了直接依据；与三种经典控制律的对照进一步凸显了预测前馈与显式约束处理在高速工况下的价值。

关键词：模型预测控制；轨迹跟踪；二自由度模型；CarSim 联合仿真；线性时变系统；二次规划；横向控制

1 引言

轨迹跟踪是无人驾驶车辆运动控制的两大核心任务之一，另一项为路径规划。横向控制器的职责，是在执行机构能力范围之内，使车辆以尽可能小的横向偏差与航向偏差跟随既定参考路径，同时保证行驶过程的平稳与乘坐舒适。围绕这一目标，学术界与工业界先后发展出几何类方法、误差反馈类方法与基于模型预测的方法三条技术路线，三者在结构复杂度、约束处理能力与高速适应性上呈现出明显的梯度差异。

经典的几何类方法以纯跟踪与 Stanley 法为代表。前者将车辆与预瞄点之间的运动关系近似为圆弧几何问题，通过预瞄距离的调度间接调节转向增益；后者则在投影点上同时补偿航向偏差与横向偏差，并借助速度归一化使横向增益随车速自动衰减。这类方法结构简洁、计算量可忽略，在低速园区场景中表现良好，但其本质是对瞬时几何关系的反馈补偿，既未显式利用车辆动力学信息，也不具备对转角幅值与转角速率的约束处理能力。当车速升高或路径曲率增大时，预瞄距离的整定变得困难，转向输出容易出现抖动，严重时甚至引发失稳。基于动力学的 PID 类方法虽然引入了误差加权与一定的动力学先验，但同样缺乏对执行器饱和与速率限制的系统性刻画，权重整定高度依赖工程经验，稳定性缺乏明确依据。

模型预测控制凭借滚动优化、反馈校正与显式约束处理三大特征，成为车辆横向控制的主流方案之一。其基本思想是：在当前采样时刻，基于车辆模型预测未来一段时域内的系统输出，在线求解一个带约束的开环最优控制序列，仅执行序列首元素，随后在下一采样时刻以更新后的状态重新优化，如此循环构成滚动时域控制。Falcone 等人较早将 MPC 应用于主动转向与制动集成控制，在极限工况下验证了其相对于几何方法的优越性。面向工程实现，线性时变 MPC 通过对非线性动力学在当前工作点逐周期线性化，在保证实时性的同时显著扩大了模型的有效工作范围，因而成为车载控制器中较为务实的折中方案。

本文依托 CarSim 2019 与 MATLAB R2022a 联合仿真环境，以二自由度横向动力学为被控对象模型，设计并实现了一套完整的 LTV-MPC 轨迹跟踪控制系统，同时保留纯跟踪、Stanley 与加权误差反馈三种对照控制器，构成统一平台下的横向控制基准对比。本文的工作可概括为三个方面：其一，给出了从非线性二自由度模型到增广线性预测模型的完整推导链条，使模型变换的每一步都具有明确的物理与数学依据；其二，将一步非线性预测残差作为扰动补偿项引入预测方程，以较小的在线代价吸收线性化误差与参数摄动；其三，从实现层面系统梳理了单位换算、容错机制、计算复杂度与参数整定规律，为同类系统的工程落地提供参考。

全文结构安排如下：第 2 节界定坐标系、符号与控制目标；第 3 节介绍参考路径生成与跟踪误差定义；第 4 节建立二自由度横向动力学模型并给出车辆参数；第 5 节详细阐述 LTV-MPC 控制器的设计流程；第 6 节介绍三种对照控制器并作横向比较；第 7 节讨论工程实现要点；第 8 节分析实时性与计算复杂度；第 9 节给出仿真结果分析与参数整定规律；第 10 节总结全文并展望后续工作。

2 坐标系、符号与问题描述

系统涉及两个坐标系。其一是固连于地面的惯性坐标系，用于描述车辆的绝对位置与航向；其二是固连于车辆质心的车体坐标系，其纵轴沿车辆纵轴指向前方，横轴指向车辆左侧，用于描述车辆自身的运动分量。车辆纵轴相对于惯性系横轴的夹角定义为横摆角，其时间导数即为横摆角速度；车体坐标系下的纵向速度与横向速度分别刻画车辆沿自身纵轴与横轴的运动；质心在惯性系中的两个坐标分量则给出车辆的绝对位置。控制输入取为前轮转角与后轮转角，纵向速度由独立的速度控制器闭环维持在恒定值，在横向控制器设计时被视为已知的时变参数而非决策变量。全文主要符号在表 1 中集中列出，以便后续推导查阅。

表 1 主要符号一览

轨迹跟踪的控制目标可表述为：在任意采样时刻，给定参考路径，求解一组控制序列，使车辆状态对参考状态的偏差在预测时域内加权最小，同时满足执行器约束。本文选取跟踪输出为横摆角与横向位置，相应的参考输出由参考路径的解析表达给出。这一输出选择兼顾了姿态跟踪与位置跟踪两个维度，且与第 4 节动力学模型的状态分量直接对应，便于预测方程的装配。

3 参考路径生成与误差定义

3.1 路径函数与样条平滑

为使跟踪任务兼顾平缓段与弯曲段，参考路径取长度为 800 米的余弦型曲线，其横向坐标随纵向坐标呈余弦变化，幅值与偏置经过选取，使路径在起点与终点附近过渡自然。在 800 个均匀采样点上取值后，通过三次样条插值将路径加密为 8001 个点的光滑点集，作为四个控制器共用的参考点集。样条插值的引入一方面消除了原始采样点之间的折线效应，使切线方向与曲率的估计更为平滑；另一方面为后续投影点搜索提供了足够密集的候选点，降低了搜索分辨率不足带来的误差。由于 MPC 额外需要参考航向，故将带航向的三列数据预存于数据文件，供控制器逐周期读取，避免在线重复计算。

3.2 解析航向与曲率

由于路径函数具有解析形式，其航向角与曲率均可直接求导获得，无需数值差分。航向角由一阶导数经反正切得到，曲率则由二阶导数按标准曲率公式计算。解析求导的好处在于：航向与曲率的估计不受采样间隔与差分格式影响，在路径曲率变化剧烈处仍能保持较高精度，从而为 MPC 的参考外推提供可靠依据。经计算，路径最大航向约为 31 度，最大曲率约为 0.006，属于中等强度弯道，既足以检验控制器在大曲率段的跟踪品质，又不至于使车辆进入轮胎非线性区，与第 4 节线性轮胎假设相自洽。

3.3 投影点搜索与跟踪误差

跟踪误差的计算依赖参考点集上的投影搜索。在当前位置处遍历参考点集，取欧氏距离最小的索引作为投影点，其切线方向由相邻参考点差分近似。航向误差定义为投影点切线角与车辆实际航向之差；横向误差取带符号的最近点距离，符号项依据投影点位于车辆纵轴哪一侧确定，从而将左偏与右偏映射为转向的正负方向。需要说明的是，车体纵轴相对惯性系的航向严格而言应为横摆角减去质心侧偏角，仿真中以航向角反馈近似，这一近似在小侧偏角假设下是合理的。投影搜索的实现还包含越界保护：当最近点位于点集末端时改用前向相邻点差分，避免索引越界导致的运行时错误。

4 二自由度横向动力学模型

4.1 建模假设

控制器内部模型采用二自由度横向动力学，其成立基于以下假设：车辆关于纵轴对称，左右轮合并为单个等效轮；轮胎工作在线性区，侧偏力与侧偏角成正比；横摆角与质心侧偏角为小量，可作线性化处理；纵向速度在预测时域内恒定，忽略纵横向动力学耦合与载荷转移；忽略转向系统传动比与执行器动态，控制量直接取为等效轮转角。上述假设的核心取舍在于：以最小的状态维数保留横摆—侧偏耦合这一决定高速跟踪稳定性的核心机理，同时舍弃对横向控制影响较弱的纵向与垂向自由度。这一取舍使模型既足以支撑预测控制的设计，又使在线线性化与离散化的计算量控制在毫秒量级。

4.2 模型方程

取横摆角、质心横向速度、惯性系横向位置与横摆角速度构成四维状态向量，取前、后轮转角构成二维控制向量。动力学方程中，横向加速度方程包含侧偏刚度与速度比值构成的阻尼项、前后轴布置差异带来的横摆耦合项、以及前后轮转角产生的前馈项；横摆角加速度方程结构类似，但前后轮的符号相反，体现了前轮转向与后轮转向对横摆运动的差异化作用；横向位置的导数由纵向速度与横摆角之积加上质心横向速度构成，其中前者体现了航向变化对横向位置的积分效应，是路径跟踪区别于车道保持的关键耦合项。将上述方程简记为状态空间形式，并取横摆角与横向位置为输出，即得到后续线性化与离散化的起点。

4.3 车辆参数

车辆与控制器参数在表 2 中集中给出。整车质量、轴距、前后轴至质心距离、横摆转动惯量与前后来侧偏刚度取自典型乘用车参数，纵向速度设为 90 km/h 以考察高速工况。控制器侧，预测时域取 20 步、控制时域取 5 步、采样周期取 20 毫秒，跟踪权重中横向位置权重比横摆角权重高两个数量级，控制增量权重取较大值以抑制转向抖动，松弛因子权重远大于跟踪权重以保证约束冲突时优先收紧松弛变量。转角限值与转角增量限值依据执行机构能力设定，前者对应机械极限，后者对应电动助力转向系统的典型带宽。

表 2 车辆与控制器参数

5 LTV-MPC 横向控制器设计

5.1 雅可比线性化

非线性模型在预测周期内被视为时不变，在当前工作点处作一阶泰勒展开，得到以雅可比矩阵描述的局部线性模型。对动力学方程逐项求偏导后，状态雅可比矩阵呈现出明确的物理结构：第三行的纵向速度项对应航向—位置积分通道，第二、四行中侧偏刚度与速度的比值项构成侧偏—横摆阻尼，前后轴布置差异则体现为交叉耦合项。控制雅可比矩阵的非零行集中于横向加速度与横摆角加速度方程，前轮与后轮在横摆方程中的符号相反，与 4.2 节的物理分析一致。线性化后通过工作点平移即可化为齐次增量模型，为后续离散化与增广建模铺平道路。需要指出，雅可比矩阵中的纵向速度取标称值而非实测值，这一简化带来的模型失配将在 5.4 节由扰动补偿项显式吸收。

5.2 前向欧拉离散化

采用前向欧拉法将连续模型离散化，离散状态矩阵为单位阵加上采样周期与连续雅可比之积，离散控制矩阵为采样周期与连续控制雅可比之积。该方法的截断误差为采样周期的平方量级。在 20 毫秒采样周期下，离散化引入的额外误差远小于线性化本身引入的模型失配，且该失配将在 5.4 节被显式补偿，因此前向欧拉法在精度与实现简洁性之间取得了合理平衡。若追求更高精度，可改用双线性变换或零阶保持器，但会相应增加在线计算量，对于本文的采样周期而言收益有限。

5.3 增广状态与增量模型

直接以控制量为优化变量会使相邻时刻转角出现跳变，既不符合转向执行机构的连续特性，也会激励未建模的高频动力学。为此将上一时刻控制量并入状态，定义增广状态向量与控制增量。增广系统的状态矩阵在原离散状态矩阵基础上扩展，控制矩阵相应扩展，输出矩阵则仅在原状态分量上取值。增广后状态矩阵的特征值在原特征值基础上增补两个单位特征值，对应控制量本身的积分通道；该积分关系由二次规划的等式结构隐式保证，无需额外施加约束。这一处理使优化变量变为控制增量，从而能够自然地对转角速率施加约束，是 MPC 转向控制中的标准做法。

5.4 一步非线性预测的扰动补偿

雅可比线性化仅在标称工作点附近有效，当车辆状态偏离工作点或模型参数存在摄动时，线性模型的预测将与真实演化产生偏差。为吸收这一偏差，在每个控制周期用原始非线性模型做一步前向预测，并将其与线性模型的预测之差定义为扰动估计。将该扰动增广后加入预测方程，即得到修正后的增广模型。这一结构本质上是线性时变 MPC 中的模型失配前馈：在每个采样点重新线性化的同时，把残差作为可测扰动一步前馈，从而在不显著增加在线计算量的前提下提升预测精度。实现中扰动在预测时域内近似为常值，即认为扰动在 0.4 秒的预测窗口内缓慢变化。该近似大幅降低了在线计算量，代价是长预测时域末端的补偿精度略有下降。值得强调的是，雅可比计算采用标称速度而实际车速更高，两者之差恰好被扰动估计吸收，这从侧面体现了扰动补偿对线性化点选择的鲁棒性。

5.5 预测方程推导

对修正后的增广模型在预测时域内逐周期递推，并将全预测时域的输出堆叠为输出向量，将控制时域内的控制增量堆叠为决策向量，即可得到预测方程的紧凑仿射形式。该形式由三部分叠加而成：初始状态经状态矩阵幂次传播得到的自由响应项、控制增量经下三角分块矩阵映射得到的受控响应项、以及扰动经传播矩阵累积得到的扰动响应项。其中受控响应矩阵为下三角分块矩阵，体现了因果性——时刻靠后的输出只受其之前控制增量的影响；扰动响应矩阵则为扰动在预测时域内的传播算子。代入本文维度后，输出预测向量为 40 维，受控响应矩阵为 40 行 10 列，扰动响应矩阵为 40 行 120 列，各矩阵的规模与预测时域、控制时域及状态维数直接相关，为第 8 节的计算复杂度分析提供了依据。

5.6 参考轨迹获取

预测控制需要未来若干步的参考输出。参考路径以纵向坐标为自变量给出，故先由车体速度向惯性系投影得到纵向速度，并假设预测时域内纵向匀速运动，外推未来纵向位置；再对预存的带航向路径数据做线性插值获得各预测步的参考横向位置与参考航向；当外推位置超出路径末端时取末点参考值，防止索引越界。插值而非最近邻取值的做法，使参考输出在预测时域内连续变化，避免了参考信号阶跃对优化解的激励。堆叠后即得到与输出预测向量同维的参考向量，供目标函数装配使用。

5.7 目标函数与二次规划装配

滚动优化目标取跟踪误差、控制增量与松弛变量的加权平方和。跟踪权重为分块对角阵，每步权重中横向位置权重比横摆角权重高两个数量级，体现了位置跟踪优先、航向平滑次之的整定取向；增量权重取较大值，强烈惩罚转角变化，配合增量硬约束共同抑制转向抖动；松弛权重远大于跟踪权重，保证约束冲突时优先收紧松弛变量而非牺牲可行性。将预测方程代入目标函数，整理为关于决策向量的二次型，舍去与决策变量无关的常数项，即得到标准二次规划形式。决策向量由控制增量序列与松弛变量拼接而成，维度为 11；海森矩阵由受控响应矩阵与跟踪权重、增量权重共同构成，线性项则由参考偏差与受控响应矩阵的乘积给出。实现中对海森矩阵做对称化处理，以满足内点法对 Hessian 对称性的数值要求，这一细节虽小，却是求解器稳定收敛的前提。

5.8 约束体系

转角幅值约束方面，前、后轮转角均受机械极限约束。引入下三角累加矩阵及其克罗内克积后，控制量沿时域的展开式可写为上一时刻控制量的时域重复加上累加矩阵与控制增量之积，于是转角约束转化为关于控制增量的线性不等式。转角速率约束方面，转向执行机构带宽有限，控制增量须落在给定区间内；在单个采样周期内，0.248 弧度的增量限值对应约 12.4 弧度每秒的等效转角速率，与电动助力转向系统的典型带宽相符。松弛变量方面，为使约束冲突时二次规划仍可行，引入松弛变量并仅对目标函数加权，不使其参与预测方程，同时给出松弛变量的上下界。实现中曾预留输出约束通道，用于对横摆角与横向位置施加容许包络，但最终出于保守性仅保留硬约束，将安全裕度交由权重组整定实现，这一取舍反映了工程实践中对约束刚性与可行性之间张力的权衡。

5.9 滚动优化与求解容错

二次规划采用内点法求解，目标容差与约束容差均设为极小值，最大迭代次数足以覆盖 11 维决策变量的搜索空间。求解成功后仅执行序列首元素，并积分更新控制量；更新后的控制量一方面经 CarSim S-Function 施加于车辆，另一方面写入全局记忆变量，在下一周期构成增广状态的控制分量。若求解器返回空解，控制器进入安全兜底逻辑，输出零转角并保持车辆直线行驶，这是车载控制器必须具备的故障降级策略。整个状态读取、线性化、预测、求解、执行流程在每个采样周期内闭环完成，构成滚动时域控制。求解耗时的在线监控为实时性评估提供了数据支撑，也为后续向嵌入式平台移植时的算力预算提供依据。

6 对照横向控制器

为在同一动力学对象与参考路径下评价 MPC 的优劣，系统同时实现了三种经典横向控制器，三者共用第 3 节的误差计算模块，从而保证比较的公平性。

6.1 统一误差计算模块

误差模块对每个周期执行投影点搜索，输出航向误差与带符号横向误差。其实现包含两个工程细节：其一，投影索引越界保护，当最近点位于点集末端时改用前向相邻点差分，避免索引越界；其二，横向误差的符号由参考点与车辆横向位置之差的符号确定，与第 3.3 节定义一致。统一误差模块的存在，使三种对照控制器与 MPC 在同一误差度量下比较，排除了误差定义差异对结论的干扰。

6.2 纯跟踪控制器

纯跟踪将车辆与预瞄点之间的关系近似为车辆后轴沿圆弧驶向预瞄点的几何问题。沿参考路径从投影点起累加弧长直至达到预瞄距离，得到预瞄点；预瞄角定义为预瞄方位与当前航向之差；由正弦定理与阿克曼几何关系，可得纯跟踪转向律。由几何关系易知，预瞄点相对车辆的横向偏移近似等于预瞄距离与预瞄角正弦之积，故该律等价于按横向偏差的比例转向，比例系数随预瞄距离调度。纯跟踪无前馈项，弯道中必然存在由曲率引起的稳态横向误差，且预瞄距离对速度敏感：预瞄距离过大则跟踪滞后，过小则航向抖动。本文取固定预瞄距离，未引入速度调度，这一简化在 90 km/h 工况下会放大其稳态误差，恰好为 MPC 的对比提供了鲜明参照。

6.3 Stanley 控制器

Stanley 律由航向补偿项与横向偏差补偿项叠加而成。第二项中速度归一化使横向增益随车速自动衰减，避免高速下小误差引发大转角；反正切函数则将补偿项天然限幅在正负二分之派之间，具备内在的有界性。与纯跟踪相比，Stanley 直接在投影点上补偿航向与横向偏差，弯道中无前馈导致的稳态误差更小，但对投影点跳变与参考路径曲率不连续更敏感，转角输出易出现高频抖动。本文取横向增益为 5，车速以千米每小时读入后归一化为米每秒参与计算，这一单位处理看似琐碎，却是联合仿真中最易出错的环节之一，第 7 节将专门讨论。

6.4 加权误差反馈控制律

第三种对照策略是一种双误差加权反馈，结构上等价于无积分通道的比例微分型控制器。控制器以后轴中心为参考点，沿当前航向取预瞄距离；参考航向不采用数值差分，而是直接利用路径解析导数，从而避免了差分噪声。控制律为航向误差与横向误差的线性加权，两个权重分别取 1 与 5。该律实现极简、计算量可忽略，但两个权重需针对车速与路径曲率人工整定，且权重与转角之间的映射依赖经验标定，缺乏约束处理能力与明确的稳定性依据。将其纳入对照体系的价值在于：它代表了工程现场最常见的快速实现方案，其性能边界恰好说明了为何高速工况需要更系统的控制设计。

6.5 三种控制律的比较

三种对照控制律与本文 MPC 的对比在表 3 中给出。从设计基础看，纯跟踪与 Stanley 源于几何关系，加权反馈源于误差线性组合，MPC 则建立在动力学预测模型之上；从前馈能力看，几何类方法或全无前馈、或仅有弱航向前馈，MPC 则通过预测与扰动补偿具备完整前馈；从约束处理看，三种对照方法均无显式约束，MPC 则对转角与转角速率施加硬约束；从高速适应性看，纯跟踪依赖预瞄距离调度，Stanley 表现较好，加权反馈高度依赖整定，MPC 因模型随速变化而适应性最好；从计算量看，三种对照方法可忽略，MPC 每周期需一次二次规划求解；从主要缺陷看，纯跟踪存在弯道稳态误差，Stanley 输出易抖动，加权反馈缺乏稳定性依据，MPC 则依赖模型精度。这一对比框架为第 9 节的仿真结论提供了结构化的解释路径。

表 3 横向控制律对比

7 工程实现要点

7.1 信号单位换算

CarSim 经 S-Function 输出的信号单位与控制器内部单位不一致，需在每周期入口完成归一化：纵向与横向速度由千米每小时除以 3.6 化为米每秒，纵向速度附加微小偏置以防止后续除零；横摆角与横摆角速度由度乘以圆周率除以 180 化为弧度。单位错配是联合仿真中最常见的隐蔽错误，其症状往往表现为控制器输出量级异常或车辆行为发散，而排查成本极高。在入口处集中换算、在模型内部统一使用国际单位制，是避免此类错误的有效工程约束。

7.2 全局控制记忆与代数环

MPC 的增广状态包含上一时刻控制量，该量以上一周期求解结果的形式存储于全局变量，在下一周期读入构成增广状态的控制分量。S-Function 设置为含直接馈通，即输出可直接依赖当前输入，从而避免在 Simulink 中形成代数环。代数环一旦形成，仿真器将被迫迭代求解，不仅拖慢仿真速度，还可能因迭代不收敛而报错。全局记忆变量的使用虽破坏了函数的纯性，但在 Simulink 的采样周期框架下是可控且常见的做法，其正确性依赖于仿真器对采样时刻的严格调度。

7.3 求解器设置与容错

求解器关闭显示输出以减少仿真日志开销，目标容差与约束容差均设为极小值，最大迭代次数足以覆盖 11 维决策变量的搜索空间。求解失败时输出零转角的安全策略，保证了极端工况下控制器的确定性行为，避免了因求解器异常导致的输出不可预期。每个周期的求解耗时由计时函数监控，为实时性评估提供数据支撑。这一套设置体现了车载控制器设计的一条基本原则：正常工况追求性能，异常工况追求确定性，二者不可偏废。

8 实时性与计算复杂度分析

二次规划的决策变量维数为 11，海森矩阵为 11 阶方阵，不等式约束共 20 行。内点法每迭代的主要开销为 Cholesky 分解与回代，量级约为决策变量维数的立方，即千次浮点运算量级；再计入状态矩阵幂次的预计算，单个周期的总计算量在毫秒量级，相对于 20 毫秒的采样周期留有充足裕量。需要指出，预测矩阵的装配采用元胞数组逐块填充后一次性拼接，虽然可读性好，但存在冗余内存分配；若用于量产嵌入式平台，可改为按行递推累乘的方式进一步压缩内存分配开销。此外，雅可比矩阵的解析形式固定，可在编译期展开为常数系数的线性组合，进一步降低在线求导开销。从实时性角度看，本文方案在桌面级仿真环境中游刃有余，向嵌入式平台移植的主要挑战不在于浮点算力，而在于内存分配策略与求解器的定点化改造。

9 仿真结果分析与参数整定

9.1 评价指标

仿真以横向误差与航向误差的示波器曲线为主要评价依据，辅以 CarSim 动画与整车状态曲线。核心指标包括：横向误差的最大绝对值、稳态段均方根、航向误差均方根、前轮转角峰值及其速率峰值，以及全程是否触发转角或增量约束的饱和。这些指标从精度、平稳性与约束满足三个维度刻画控制器性能，与第 6.5 节的对比框架相互呼应。

9.2 参数影响规律

各参数对闭环性能的影响可归纳为以下规律。预测时域方面，时域越大，控制器对弯道曲率的预见性越强，进入弯道前的预转向越充分，但扰动常值近似与线性化误差也随之累积，过大的时域反而降低末端预测精度；本文取 20 步即 0.4 秒，约为高速工况下两到三倍预瞄时间的合理取值。控制时域方面，其决定控制增量的自由度，进而影响求解规模与转角平滑性；取 5 步时转角序列有足够自由度完成预转向与回正动作，继续增大对跟踪精度的边际贡献有限，却显著增加二次规划耗时。跟踪权重方面，横向位置权重主导位置跟踪精度，其增大会使进入弯道的纠偏更激进，但转角与横摆响应更剧烈；横摆角权重过小会导致车身姿态摆动，过大则位置收敛变慢。增量权重方面，其是抑制转向抖动的首要手段，但过大的增量权重会削弱纠偏速度，表现为弯道中横向误差的持续积累；本文取较大增量权重并与增量硬约束配合，构成软惩罚加硬限幅的双层速率管理。松弛参数方面，松弛权重须远大于跟踪权重才能使松弛变量仅在约束冲突时非零；松弛上界给出扰动允许的最大违反量，相对于转角限值是宽松的，正常情况下松弛变量近似为零。

9.3 控制器对比的预期结论

在中等曲率、90 km/h 工况下可预期：纯跟踪因缺少曲率前馈，弯道内存在可见的稳态横向误差，且误差随车速上升而恶化；Stanley 的横向误差收敛最快，但转角曲线毛刺明显；加权反馈的性能介于两者之间，且对权重整定高度敏感；LTV-MPC 凭借预测前馈与扰动补偿，在入弯前即可产生预转向，横向误差峰值与稳态值均最小，同时转角受硬约束保护、变化平滑，综合性能最优。横向与航向误差的示波器曲线为该对比提供了直接的量化依据。需要说明的是，上述结论建立在模型参数与实际车辆匹配良好的前提之下；若轮胎进入非线性区或路面附着系数显著下降，线性模型的预测精度将下降，此时 MPC 的优势可能收窄，这正是后续工作引入轮胎非线性模型与输出软约束的动机所在。

10 结论

本文完整解析了一套基于 CarSim/Simulink 联合仿真的 LTV-MPC 车辆轨迹跟踪系统，主要结论如下。其一，所采用的雅可比线性化、前向欧拉离散、增广增量建模与一步非线性残差补偿相结合的技术路线，以每周期一次 11 维二次规划的代价，实现了对二自由度非线性横向动力学的有效预测控制，20 毫秒采样周期下实时性充足。其二，显式的转角幅值约束与转角速率约束使控制器输出天然符合执行机构特性；松弛变量设计保证了约束冲突时的可行性，求解失败时的零转角兜底策略提供了功能安全降级路径。其三，以纯跟踪、Stanley 与加权误差反馈为对照的基准体系表明，几何类控制器结构简单但缺乏约束处理与前馈能力，MPC 通过预测前馈与扰动补偿在跟踪精度与行驶平顺性上取得更优折中。

后续工作可在三个方向拓展。其一，将纵向通道纳入统一 MPC 框架，实现横纵向协同优化，从而在弯道中通过速度调节进一步降低横向误差。其二，启用预留的输出软约束并引入轮胎非线性模型，以覆盖极限工况下的附着饱和与侧偏非线性。其三，将逐周期重复装配的预测矩阵改为增量递推更新，并向嵌入式平台移植，重点解决内存分配策略与求解器定点化问题。这三个方向分别对应性能边界、模型精度与工程落地三个层面，构成了从仿真验证走向实车部署的渐进路径。

📚第二部分——运行结果

部分代码：

function [sys,x0,str,ts] = MPC_4WS(t,x,u,flag)

% Q_40*40,R_5*5,a_6*6,b_6*1,d_k 6*1,A_7*7,B_7*1,C_2*7
% Nx=6;%状态量的个数；Nu=1;%控制量的个数；Ny=2;%输出量的个数；Np =20;%预测时域；Nc=5;%控制时域
% 状态量=[phi,y_dot,Y,phi_dot]，控制量为前轮偏角[delta_f,delta_r]

switch flag
case 0
[sys,x0,str,ts] = mdlInitializeSizes; % Initialization

case 2
sys = mdlUpdates(t,x,u); % Update discrete states

case 3
sys = mdlOutputs(t,x,u); % Calculate outputs

% case 4
% sys = mdlGetTimeOfNextVarHit(t,x,u); % Get next sample time

case {1,4,9} % Unused flags
sys = [];

otherwise
error(['unhandled flag = ',num2str(flag)]); % Error handling
end
% End of dsfunc.

%==============================================================
% Initialization
%==============================================================

function [sys,x0,str,ts] = mdlInitializeSizes

% Call simsizes for a sizes structure, fill it in, and convert it
% to a sizes array.

sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 4;
sizes.NumOutputs = 2;
sizes.NumInputs = 8;
sizes.DirFeedthrough = 1; % Matrix D is non-empty.
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0 =[0.001;0.0001;0.0001;0.00001];
global U;%U为我们的控制量
U=[0, 0];%

% Initialize the discrete states.
str = []; % Set str to an empty matrix.
ts = [0.02 0]; % sample time: [period, offset]，采样时间影响最大。

global VehiclePara;
VehiclePara.m = 1412; %m为车辆质量,Kg; Sprung mass = 1370
VehiclePara.g = 9.8;
VehiclePara.Lf = 1.015; % 1.05
VehiclePara.Lr = 1.895; % 1.55
VehiclePara.L = 2.91; %VehiclePara.Lf + VehiclePara.Lr;
VehiclePara.Iz = 1536.7; %I为车辆绕Z轴的转动惯量，车辆固有参数
VehiclePara.Ccf = 52000;
VehiclePara.Ccr = 34500;
VehiclePara.Clf = 52000;
VehiclePara.Clr = 34500;

global MPCParameters;
MPCParameters.Ts = 0.02; % the sample time of near term

% Update the discrete states
function sys = mdlUpdates(t,x,u)

sys = x;
%End of mdlUpdate.

%==============================================================
% Calculate outputs
%==============================================================
function sys = mdlOutputs(t,x,u)
global VehiclePara;
global MPCParameters;
global U; %设置U为全局量
tic
Nx=4;%状态量的个数
Nu=2;%控制量的个数
Ny=2;%输出量的个数
Np=20;%预测时域
Nc=5;%控制时域
Row=1000;%松弛因子权重
fprintf('Update start, t=%6.3f\n',t)

% 反馈状态量
y_dot=u(1)/3.6; %横向速度化为m/s
x_dot=u(2)/3.6+0.0001;%CarSim输出的是km/h，转换为m/s
phi=u(3)*pi/180; %CarSim输出的为角度，角度转换为弧度，使用弧度
phi_dot=u(4)*pi/180;%角速度
Y=u(5);%单位为m，横向位置
X=u(6);%单位为米，纵向位置

% %% 车辆参数输入
%syms sf为前轮滑移率，sr为后轮滑移率
Sf= u(7); Sr= u(8);
%syms lf%前轮距离车辆质心的距离，lr为后轮距离车辆质心的距离
Lf=1.015;Lr=1.895;
%syms C_cf前轮线性横向侧偏刚度； C_cr后轮线性横向侧偏刚度 ；C_lf 前轮纵向侧偏刚度； C_lr 后轮纵向侧偏刚度
Ccf=52000;Ccr=34500;Clf=52000;Clr=34500;
%syms m g I;%m为车辆质量，g为重力加速度，I为车辆绕Z轴的转动惯量，车辆固有参数
m=1412;g=9.8;Iz=1536.7;
%

%% 参考轨迹生成
X_predict=zeros(Np,1);%用于保存预测时域内的纵向位置信息，这是计算期望轨迹的基础
phi_ref=zeros(Np,1);%用于保存预测时域内的参考横摆角信息
Y_ref=zeros(Np,1);%用于保存预测时域内的参考横向位置信息
load path5_mpc

% 以下计算状态量，即状态量与控制量合在一起
kesi=zeros(Nx+Nu,1);%状态量4个，控制量2个。初始化一个大小为Nx+Nu的零向量，其中Nx是状态量的数量，Nu是控制量的数量。
kesi(1)=phi;%将当前横摆角phi赋值给状态量向量的第一个元素。
kesi(2)=y_dot;%将当前横向速度y_dot赋值给状态量向量的第二个元素。
kesi(3)=Y; %将当前横向位置Y赋值给状态量向量的第三个元素。
kesi(4)=phi_dot;%将当前横摆角速度phi_dot赋值给状态量向量的第四个元素。
kesi(5)=U(1);%将控制量向量U的第一个元素赋值给状态量向量的第五个元素。
kesi(6)=U(2); %这个是控制量前轮偏角，即在轨迹跟踪过程中通过对前轮偏角进行控制，纵向速度保持不变
delta_f=U(1); %前轮转角
delta_r=U(2); %前轮转角
% fprintf('Update start, u(1)=%4.2f\n',U(1))
vx = 90/3.6; %速度设置
T=0.02;%仿真步长
T_all=100;%总的仿真时间，主要功能是防止计算期望轨迹越界

%权重矩阵设置
Q_cell=cell(Np,Np);%总的元胞Q为20行20列的，40*40
for i=1:1:Np
for j=1:1:Np
if i==j
Q_cell{i,j}=[2000 0;0 100000;]; %作用是将方阵对角线的地方设置为该矩阵，其余为止全部为0
else
Q_cell{i,j}=zeros(Ny,Ny); %20行20列的矩阵中每一个小的位置上都是2*2的小矩阵
end
end
end
R=5*10^5*eye(Nu*Nc);

%采用动力学模型，该矩阵与车辆参数密切相关，通过对动力学方程求解雅克比矩阵得到，a为6*6，b为6*1
[a, b] = func_Model_linearization_Jacobian1(kesi,Sf,Sr,MPCParameters,VehiclePara);

d_k=zeros(Nx,1);%计算偏差,4*1
state_k1=zeros(Nx,1);%预测下一时刻状态量，用于计算偏差，4*1
%以下即为根据离散非线性模型预测下一时刻状态量
%注意，为避免前后轴距的表达式（a,b）与控制器的a,b矩阵冲突，将前后轴距的表达式改为lf和lr，下面这些公式见（P35）
%自主车辆线性时变模型预测路径跟踪控制
state_k1(1,1)=phi+T*phi_dot;
state_k1(2,1)=y_dot+T*(-(Ccf+Ccr)*y_dot/(m*vx) - (Lf*Ccf-Lr*Ccr)*phi_dot/(m*vx) + vx*phi_dot + Ccf*delta_f/m + Ccr*delta_r/m);
state_k1(3,1)=Y+T*(vx * phi + y_dot);
state_k1(4,1)=phi_dot+T*(-(Lf*Ccf-Lr*Ccr)*y_dot/(Iz*vx) - (Lf^2*Ccf+Lr^2*Ccr)*phi_dot/(Iz*vx) + Lf*Ccf*delta_f/Iz - Lr*Ccr*delta_r/Iz);
%到此为止，下一时刻的状态量已经推导完毕
d_k=state_k1-a*kesi(1:4,1)-b*kesi(5:6,1);
d_piao_k=zeros(Nx+Nu,1);%偏差矩阵d_k的增广形式，又加了控制量的那2行，维度6*1
d_piao_k(1:4,1)=d_k; %给d_k的增广形式进行赋值，前6行为我们的偏差
d_piao_k(5:6,1)=[0;0];

A_cell=cell(2,2);
B_cell=cell(2,1);
A_cell{1,1}=a;%第一行第一列4*4
A_cell{1,2}=b;%第一行第二列为4*2
A_cell{2,1}=zeros(Nu,Nx);%第二行第一列为1*6
A_cell{2,2}=eye(Nu);%最二行第二列为1*1
B_cell{1,1}=b;%B矩阵第一行第一列为b,4*2
B_cell{2,1}=eye(Nu);%第二行第一列为2*2单位阵
A=cell2mat(A_cell);%A矩阵维度7*7
B=cell2mat(B_cell);%B矩阵维度7*1
C=[1 0 0 0 0 0;0 0 1 0 0 0;];%我们只输出状态看空间的第1个量（横摆角）和第3个量（横向位置Y）

PSI_cell=cell(Np,1);%输出方程的第一个系数矩阵，维度 20*1
THETA_cell=cell(Np,Nc);%输出方程的第二个系数矩阵，维度 20*5
GAMMA_cell=cell(Np,Np);%维度 20*20
PHI_cell=cell(Np,1);%维度 20*1

for p=1:1:Np
PHI_cell{p,1}=d_piao_k;% 6行1列的偏差，理论上来说，这个是要实时更新的，但是为了简便，这里又一次近似
for q=1:1:Np
if q<=p %下三角矩阵
GAMMA_cell{p,q}=C*A^(p-q); %该矩阵由C和A构造而成
else
GAMMA_cell{p,q}=zeros(Ny,Nx+Nu); %每一个元胞的大小为2（输出两个数）*7（状态空间量的个数）
end
end
end %最终该矩阵为40*140

for j=1:1:Np %j从1到20
PSI_cell{j,1}=C*A^j; %构造输出方程式（21）中第一项的系数矩阵，2*7*7*7=2*7
for k=1:1:Nc %k从1到5，构造输出方程式（21）中第二项的系数矩阵
if k<=j %下三角的区域有值
THETA_cell{j,k}=C*A^(j-k)*B;
else %上三角区域全部为0
THETA_cell{j,k}=zeros(Ny,Nu); %每一个小元胞都是2*1小矩阵
end
end
end

PSI=cell2mat(PSI_cell);%size(PSI)=[Ny*Np Nx+Nu]，共20行1列，每一个小元胞都是2*7的小矩阵，所以为40*7
THETA=cell2mat(THETA_cell);%size(THETA)=[Ny*Np Nu*Nc]，40*5

GAMMA=cell2mat(GAMMA_cell);%大写的GAMMA；维度40*140
PHI=cell2mat(PHI_cell);%由于该元胞数组每一个位置都是7*1的矩阵，一共20行，所以一共是140*1
%144行已经求得Q元胞数组，现在将其转换成矩阵形式
Q=cell2mat(Q_cell); %总的元胞Q为20行20列的，每个位置都是2*2的小矩阵，Q最终为40*40
%构造二次规划的H矩阵
H_cell=cell(2,2);
H_cell{1,1}=2*(THETA'*Q*THETA+R);%第一行第一列的位置为5*5的矩阵
H_cell{1,2}=zeros(Nu*Nc,1);%第一行第二列的位置为5*1
H_cell{2,1}=zeros(1,Nu*Nc);%第二行第一列的位置为1*5
H_cell{2,2}=2*Row;%第二行第二列为一个值：松弛因子
H=cell2mat(H_cell);%构造完毕之后将H元胞数组转化为矩阵，最终为H为6*6
H=(H+H')/2;
error_1=zeros(Ny*Np,1);%40*7*7*1=40*1
Yita_ref_cell=cell(Np,1);%参考的元胞数组为20*1
for p=1:1:Np
X_DOT=x_dot*cos(phi)-y_dot*sin(phi);%惯性坐标系下纵向速度
X_predict(p,1)=X+X_DOT*p*T;%首先计算出未来X的位置，
%参考路径信息包括参考的纵向位置，横向位置，横摆角
last=size(path5,1);
if X_predict(p,1)>=path5(last,1)
Y_ref(p,1)=path5(last,2);
phi_ref(p,1)=path5(last,3);
Yita_ref_cell{p,1}=[phi_ref(p,1);Y_ref(p,1)];
else
x_ref=path5(:,1); %设置的纵向位移
y_ref=path5(:,2); %设置的横向位移
phi_ref=path5(:,3);
Y_ref(p,1)=interp1(x_ref,y_ref,X_predict(p,1));
phi_ref(p,1)=interp1(x_ref,phi_ref,X_predict(p,1));
Yita_ref_cell{p,1}=[phi_ref(p,1);Y_ref(p,1)];
end
end
Yita_ref=cell2mat(Yita_ref_cell);%将我们得到的输出元胞转换为矩阵
error_1=Yita_ref-PSI*kesi-GAMMA*PHI;
g_cell=cell(1,2);
g_cell{1,1}=2*error_1'*Q*THETA;
g_cell{1,2}=0;
g=-cell2mat(g_cell);
f=g';

%% 以下为约束生成区域
%控制量约束
A_t=zeros(Nc,Nc);
for p=1:1:Nc
for q=1:1:Nc
if q<=p %下三角矩阵包含对角线
A_t(p,q)=1;
else
A_t(p,q)=0;
end
end
end
A_I=kron(A_t,eye(Nu));%求克罗内克积
Ut=kron(ones(Nc,1),U);
umin=[-0.3744,-0.3744];%维数与控制变量的个数相同，前轮偏角的上约束
umax=[0.3744,0.3744];%前轮偏角的下约束
Umin=kron(ones(Nc,1),umin);
Umax=kron(ones(Nc,1),umax);

%输出量约束
ycmax=[4.2;20]; %横摆角和纵向位移的约束
ycmin=[-4.2;-20];
Ycmax=kron(ones(Np,1),ycmax);
Ycmin=kron(ones(Np,1),ycmin);

% %二次规划A矩阵
% A_cons_cell={A_I zeros(Nu*Nc,1);-A_I zeros(Nu*Nc,1);THETA zeros(Ny*Np,1);-THETA zeros(Ny*Np,1)};
% A_cons=cell2mat(A_cons_cell);%（求解方程）状态量不等式约束增益矩阵，转换为绝对值的取值范围
%
% %二次规划的b矩阵
% b_cons_cell={Umax-Ut;-Umin+Ut;Ycmax-PSI*kesi-GAMMA*PHI;-Ycmin+PSI*kesi+GAMMA*PHI};
% b_cons=cell2mat(b_cons_cell);%（求解方程）状态量不等式约束的取值

%二次规划A矩阵
A_cons_cell={A_I zeros(Nu*Nc,1);-A_I zeros(Nu*Nc,1);};
A_cons=cell2mat(A_cons_cell);%（求解方程）状态量不等式约束增益矩
% 阵，转换为绝对值的取值范围

%二次规划的b矩阵
b_cons_cell={Umax-Ut;-Umin+Ut;};
b_cons=cell2mat(b_cons_cell);%（求解方程）状态量不等式约束的取值

%控制增量约束
M=10;
delta_umin=[-0.248;-0.248];%前轮偏角变化量的下约束
delta_umax=[0.248;0.248];%前轮偏角变化量的上约束
delta_Umin=kron(ones(Nc,1),delta_umin);
delta_Umax=kron(ones(Nc,1),delta_umax);
lb=[delta_Umin;0];%（求解方程）状态量下界，包含控制时域内控制增量和松弛因子
ub=[delta_Umax;M];%（求解方程）状态量上界，包含控制时域内控制增量和松弛因子

%% 开始求解过程
%% 开始求解过程
options = optimset('Display','off', ...
'TolFun', 1e-8, ...
'MaxIter', 2000, ...
'Algorithm', 'interior-point-convex', ...
'FinDiffType', 'forward', ...
'RelLineSrchBnd', [], ...
'RelLineSrchBndDuration', 1, ...
'TolConSQP', 1e-8);
warning off all % close the warnings during computation
% x_start=zeros(Nc+1,1);%加入一个起始点
[X,~,exitflag]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,[],options);
fprintf('exitflag=%d\n',exitflag);
fprintf('H=%4.2f\n',H(1,1));
fprintf('f=%4.2f\n',f(1,1));
%% 计算输出
if isempty(X)
U(1)=0;
U(2)=0;
else
fprintf("X(1)=%4.2f\n",X(1));
u_piao1=X(1);%得到控制增量
u_piao2=X(2);
U(1)=kesi(5,1)+u_piao1;%当前时刻的控制量为上一刻时刻控制+控制增量
U(2)=kesi(6,1)+u_piao2;
sys= U;
toc
end

🎉第三部分——参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。(文章内容仅供参考，具体效果以运行结果为准)

​​​​​​🌈第四部分——本文完整资源下载

资料获取，更多粉丝福利，MATLAB|Simulink|Python|数据|文档等完整资源获取

本文完整资源下载