简介本资源是一份面向本科毕业设计与控制类课程作业的Matlab实践项目聚焦线性二次调节器LQR在车辆轨迹跟踪中的完整实现帮助学习者将控制理论转化为可运行、可验证的闭环控制系统。压缩包共9个文件含4个.mat数据文件存储预设路径、横向误差等关键轨迹与状态数据、2个.m脚本主程序main.m与圆形路径生成函数CircleGen.m、2个.png结果图直观展示跟踪效果及1份README.md说明文档整体仅107KB轻量易读、结构清晰。已有474人学习下载适合作为控制工程入门到进阶的典型范例。读者可直接运行复现LQR控制器设计全流程从车辆线性化状态空间建模、Q/R权重调参、Riccati方程求解到反馈增益K计算与实时轨迹跟踪仿真代码注释详尽变量命名规范便于理解算法逻辑与调试思路。 如果你也在做车辆横向控制或者打算用Matlab跑一段轨迹跟踪demoLQR线性二次型调节器应该是跳不过去的一个方法。我最近整理了一套Matlab实现LQR车辆轨迹跟踪的完整源码从车辆运动学建模、参考轨迹生成、误差模型推导到闭环仿真一次性跑通。这篇文章就把这套源码的设计思路、核心代码和调参坑位都拆开讲一遍适合刚接触路径跟踪控制、想快速复现并理解原理的朋友。先说结论LQR在低速和小侧偏角场景下做横向跟踪效果非常可靠计算量又小跑起来比MPC轻快得多也比PID更容易拧出理想的阻尼感。源码包里既包含了直线、圆形和双移线三种参考轨迹也放了完整的控制器、车辆更新和可视化代码改动参数就能直接演示。下面我按自己的实现顺序从方案选型开始逐步复盘。1. 项目整体设计与方案选型1.1 这个源码包到底做了什么在这套源码里我把实验环境限定在一个平面运动场景车辆被抽象成前轮转向的刚体只能前进不能横移参考轨迹通过离散点序列给出沿轨迹前进时每一帧都计算车辆当前点与参考点的横向偏差和航向偏差然后用LQR求出一个前轮转角控制量作用到车辆模型上形成闭环。仿真结果用时间序列曲线和车辆跟随动画同时展示能直观看到横向误差收敛过程。一句话总结就是给定一条参考路径车辆从任意初始位置出发通过LQR控制前轮转角最终稳定地沿路径行驶。适用对象很明确做课程设计、毕业设计或者项目预研的在校同学以及刚开始接触车辆横向控制的工程师。源码包里各模块是拆开的main主程序负责串联每一块都能单独改参数重新跑不是一锅炖的脚本。1.2 为什么选LQR而不是PID或MPC很多朋友上来就问轨迹跟踪不是用PID就行吗确实可以但PID在车辆横向控制里有个很尴尬的问题横向误差和航向误差是相互耦合的车辆当前横向偏了1米到底该给多大的转角需要同时考虑航向偏差是多少。PID要分别对两个误差独立调参很难把这种耦合关系理清楚弯道稍微急一点参数就得重新撸一遍。MPC效果确实好能处理约束还能做预测但代价是优化求解的计算量大代码复杂度也高。在低速轨迹跟踪这个场景里用MPC属于杀鸡用牛刀。LQR刚好卡在中间它基于线性模型能直接解析出最优状态反馈矩阵代码只要几行运行速度极快非常适合嵌入式部署。对于没有强烈约束需求的低速控制场景LQR是性价比最高的选择。LQR也有自己的局限它本质是线性控制器车辆模型是强非线性的。但只要把工作点限制在低速、小转角、小侧偏角范围内线性化误差完全可以接受。这也是我在这套源码里坚持用运动学模型的原因之一后面会细说。1.3 车辆模型选择运动学模型打底车辆控制里常见的模型有两类运动学模型和动力学模型。运动学模型把车辆看成一个几何体不考虑轮胎受力只描述位置、航向和速度之间的关系动力学模型则要引入轮胎侧偏刚度、横摆转动惯量、质心位置等参数描述的是车辆在受力情况下的运动响应。我这套源码采用的是运动学自行车模型也是路径跟踪入门最经典的模型。它的状态量是车辆位置 (X, Y) 和航向角 φ控制量是车速 v 和前轮转角 δ。状态方程写成dX/dt v * cos(φ) dY/dt v * sin(φ) dφ/dt v * tan(δ) / L这里的 L 是轴距。为什么选这个模型第一低速工况下轮胎侧偏角很小运动学模型和动力学模型的结果差异很小没必要引入一堆需要标定的参数第二代码简洁读者能快速抓住LQR这条主线第三整个模型只需要标定一个轴距参数换个车改一个数就行。对于教学演示和预研验证来说运动学模型已经足够用了。当然后面也有扩展空间如果车速提到十几米每秒以上我建议换动力学模型。这个在后面的扩展小节里会提到。2. LQR控制器原理与误差模型推导2.1 LQR的核心数学过程LQR全称Linear Quadratic Regulator直译是线性二次型调节器。它的前置条件是被控系统可以用线性状态方程描述dx/dt A x B u其中 x 是状态向量u 是控制量A 是系统矩阵B 是输入矩阵。控制目标是设计状态反馈 u -Kx使得系统能稳定收敛到零点同时让下面这个二次型性能指标最小J ∫(x^T Q x u^T R u) dtQ 是状态权重矩阵R 是控制权重矩阵。Q 越大说明越强调状态误差要小R 越大说明越希望控制量不要太大。性能指标里既包含状态偏差又包含控制能量这是LQR“最优”二字的由来。最优反馈增益 K 是通过求解Riccati方程得到的具体细节不展开但Matlab里可以直接用一行命令[K, S, e] lqr(A, B, Q, R);返回的 K 就是最优反馈矩阵S 是Riccati方程的解e 是闭环系统的极点。实际使用中我只需要 K 就行。lqr函数内部调用的是care即连续代数Riccati方程求解器计算一次只要毫秒级时间。这也是LQR适合实时系统的重要原因。有些朋友会问为什么不用极点配置极点配置也能算出反馈增益但极点位置需要反复试凑物理意义也不直观。LQR的 Q 和 R 矩阵能直观表达“哪个状态误差更重要、控制量能耗多少”调参时心智负担小很多。2.2 车辆横向跟踪误差模型的推导把LQR用到车辆轨迹跟踪上核心是把车辆模型转换成误差状态方程。这一步是源码里最容易懵的地方也是网上很多代码没讲清楚的部分。首先定义车辆当前位姿为 (x, y, φ)在参考轨迹上找到最近点 (x_ref, y_ref, φ_ref)。然后定义横向误差 e1 和航向误差 e2e1 -sin(φ_ref) * (x - x_ref) cos(φ_ref) * (y - y_ref) e2 φ - φ_ref也就是说e1 是车辆位置相对于参考点在横向方向上的偏差e2 是航向角偏差。对这两个误差求导联立车辆运动学方程整理后可以得到误差状态方程d(e1)/dt v * e2 d(e2)/dt v * tan(δ) / L - ω_ref其中 ω_ref 是参考轨迹的角速度即 φ_ref 的变化率。在参考曲率较小或者航向角变化缓慢的路段可以近似认为 ω_ref 的影响由前馈环节补偿误差模型简化成d(e1)/dt v * e2 d(e2)/dt v * δ / L写成矩阵形式d/dt [e1; e2] [0, v; 0, 0] [e1; e2] [0; v/L] δ于是得到状态矩阵 A [0, v; 0, 0]输入矩阵 B [0; v/L]状态向量 x [e1; e2]控制量 u δ。LQR控制率就是δ -K * [e1; e2]这里有一个非常关键的细节A矩阵和B矩阵里都包含车速 v所以车速变化会导致系统模型变化。这也是为什么我在源码里把车速设置成定值在仿真启动时一次性计算K中途不再变化。如果你希望仿真过程中变速那就得在每个时间步重新计算K或者用不同速度档位预计算K再查表。2.3 权重矩阵Q和R的整定经验Q 和 R 的选取是LQR调参的重头戏也是最多人卡住的地方。我的经验是别一上来就追求数学上的最优先按物理意义设初始值再根据仿真曲线微调。Q 矩阵针对状态量对于本文的二状态模型Q(1,1) 对应横向误差Q(2,2) 对应航向误差。R 对应前轮转角。Q(1,1) 决定车辆贴线的优先级Q(2,2) 决定车头转正的快慢R 决定转向角度是否激烈。三者的矛盾关系很明显Q(1,1) 太大会导致车辆快速打方向修正误差R 太小会让转角命令像方波一样来回抖。我常用的初始参数组合参数初始值作用说明Q(1,1)50横向误差权重Q(2,2)10航向误差权重R(1,1)1前轮转角权重v5 m/s仿真车速L2.5 m车辆轴距dt0.05 s仿真步长用这套参数跑完一圈圆形轨迹后再观察两条曲线横向误差收敛速度和前轮转角幅度。如果横向误差收敛太慢优先调大 Q(1,1)如果转角抖得厉害优先调大 R(1,1)。Q(2,2) 对动态响应影响更明显航向误差权重太高会让车辆“急于转正”容易产生振荡一般保持与 Q(1,1) 差不多的量级就好。3. 完整源码实现与仿真3.1 代码结构总览源码包里的文件结构如下lqr_trajectory_tracking/ ├── main.m ├── generate_reference_path.m ├── lqr_controller.m ├── vehicle_model.m ├── simulation_loop.m └── plot_results.mmain.m 是主入口负责初始化参数、调用轨迹生成、构建A和B矩阵、求解K、进入仿真循环、最后绘图。其他文件各司其职这样拆分的好处是每一块都可以单独调试比如你想验证轨迹生成是否正确单独跑一下 generate_reference_path 并画图就行不用牵扯控制逻辑。这种拆分方式也方便后续扩展。比如你打算把运动学模型换成动力学模型只要重写 vehicle_model.m主程序基本不用动。如果想把LQR换成MPC只需新增一个控制求解器替换 lqr_controller.m 的接口输出即可。3.2 参考轨迹生成参考轨迹我用函数生成支持直线、圆形和双移线三种方式。所有轨迹函数统一返回三列数组path_x、path_y、path_yaw分别是参考点的x坐标、y坐标和航向角。圆形轨迹的生成代码很简单function [path_x, path_y, path_yaw] generate_circle_path(R, center, num_points) theta linspace(0, 2*pi, num_points); path_x center(1) R * cos(theta); path_y center(2) R * sin(theta); path_yaw theta pi/2; end这里最需要注意的是 path_yaw 的计算。圆的参数方程里位置向量随θ变化的方向正好是圆的切线方向而切线方向与径向垂直所以航向角是在θ的基础上加90度也就是 π/2。如果不加这个偏置控制器会一直以为车头方向指向圆心轨迹跟踪肯定不对。双移线轨迹是模拟变道工况的标准测试路径用来检验控制器在连续两次反向变道时的动态响应。它由两个S形曲线叠加而成核心公式是Y(X) d1 / (1 exp(-a1 * (X - c1))) - d2 / (1 exp(-a2 * (X - c2)))其中每个sigmoid项代表一次平滑的横向位移两个项叠加就能产生先向左、再向右回到原方向的双移线效果。曲线参数决定了换道的幅度和急缓程度。双移线对控制器的考验在于曲率连续变化且存在方向突变比圆形轨迹更容易暴露参数问题。无论哪种轨迹在仿真主循环里都需要实时找到车辆当前位置的最近参考点。我采用的是暴力遍历距离取最小距离的索引。这种方法的复杂度是O(n)在离线仿真中完全够用代码也好理解。如果轨迹点有上万个可以考虑用KD树加速但这里没有必要。3.3 核心控制循环实现仿真主循环里每个时间步要做四件事找最近参考点、计算误差、计算控制量、更新车辆状态。下面是我在源码里写的主循环片段逻辑做了精简但核心过程保留。for t 0 : dt : sim_time % 1. 找到最近参考点 dist2 (path_x - x).^2 (path_y - y).^2; [~, idx] min(dist2); % 2. 计算横向误差和航向误差 dx x - path_x(idx); dy y - path_y(idx); e1 -sin(path_yaw(idx)) * dx cos(path_yaw(idx)) * dy; e2 wrapToPi(yaw - path_yaw(idx)); % 3. 计算前轮转角 delta_ff atan(L * kappa_ref(idx)); % 前馈量 delta_fb -K * [e1; e2]; % LQR反馈量 delta delta_ff delta_fb; delta min(max(delta, -0.5), 0.5); % 转角限幅 % 4. 更新车辆状态运动学模型欧拉积分 x x v * cos(yaw) * dt; y y v * sin(yaw) * dt; yaw yaw (v / L) * tan(delta) * dt; % 保存历史数据 history_x(idx_step) x; history_y(idx_step) y; history_delta(idx_step) delta; history_e1(idx_step) e1; history_e2(idx_step) e2; idx_step idx_step 1; end关于 wrapToPiMatlab的 Mapping Toolbox 里有现成函数但为了不依赖额外工具箱我在源码里写了一个自定义版本把角度归一到 [-pi, pi] 区间。这个处理很重要否则航向角误差在 ±π 边界附近会突然跳变控制器会以为误差从0.1瞬间变成6.28直接给你一个离谱的转角。控制量里加 delta_ff 前馈量是我在实际调参中加上去的。一开始我只用 LQR 反馈项圆形轨迹跑出来总有一个固定的横向稳态误差后来发现这是弯道曲率带来的必然结果。加上基于参考路径曲率的前馈转角后横向误差能直接降到接近零。前馈的公式是delta_ff atan(L * kappa)其中 kappa 是参考路径上的曲率圆形轨迹 kappa 1/R直线 kappa 0双移线 kappa 需要根据轨迹的解析表达式求导计算。3.4 仿真结果与原理解读以圆形轨迹为例车辆初始位置放在距离圆轨道横向偏移1米的地方。仿真开始后横向误差 e1 会迅速收敛大约2秒内降到厘米级后面基本稳定。控制量在前1秒有一个明显的修正转角随后趋于平缓与前馈量基本重合。双移线轨迹上横向误差会在两次换道的起始和结束位置出现两个峰值这是曲率突变导致的动态跟踪滞后属于正常现象。如果峰值过大可以适当增大 Q(1,1)但要注意这会让控制量在换道瞬间更猛烈需要同时增大 R(1,1) 或加入转角变化率限制来平抑。我把 Q(1,1) 从10调到100做了一个对比实验数据趋势非常典型Q(1,1)横向误差峰值控制量峰值收敛时间10约0.35 m约8度较慢50约0.18 m约12度中等100约0.10 m约18度较快这个表印证了LQR的本质状态权重越高系统越“激进”误差收敛更快但控制代价更大。你需要根据实际场景决定到底偏向哪边而不是一味追求误差最小。4. 常见问题与调参避坑实录4.1 控制量抖动和超调这是最常遇到的问题现象是车辆在最开始大幅打方向然后来回摆动横向误差曲线像衰减很慢的正弦波。原因一般有三个Q(2,2)过大导致航向误差反馈过强R过小导致控制量不受约束或者仿真步长太大导致离散化误差明显。解决办法按优先级来第一把 R(1,1) 调大比如从1调到5看转角曲线是否平滑第二降低 Q(2,2)航向误差权重太高容易让车头频繁修正第三把仿真步长从0.05s改成0.02s排除积分误差的干扰。如果车辆在直线段一切正常、到弯道就开始抖优先检查前馈量有没有加错符号或者参考轨迹的航向角是否存在跳变。4.2 圆形轨迹跟踪出现明显稳态误差如果只靠LQR反馈控制跑圆形轨迹几乎一定会出现一个固定方向的横向偏差。这个偏差不是参数没调好而是控制器结构决定的。LQR反馈项只有在误差存在时才会输出控制量而弯道维持转向本身就需要一个持续的基准转角这个“稳态需求”靠纯反馈是追不上的于是系统只能靠稳定误差来维持控制输出。解决办法就是加前馈。前馈量根据参考曲率直接给出基准转角反馈项只负责修正偏差这样一来稳态误差就能被消除。圆形轨迹曲率恒定前馈转角也是恒值双移线曲率变化前馈转角跟随变化。加入前馈后横向误差峰值和稳态值都能下降80%以上实测效果极其明显。4.3 LQR矩阵求解失败运行lqr(A, B, Q, R)时偶尔会报矩阵奇异之类的错误。这通常不是LQR函数本身的问题而是模型的可控性问题。比如状态选择了两个完全独立的位置量但控制量无法独立影响它们系统就不满足可控性条件。排查思路很直接先检查可控性矩阵的秩Co ctrb(A, B); if rank(Co) size(A, 1) disp(系统不可控请检查模型); end本文的二状态误差模型状态维度是2控制量维度是1rank(Co)必须等于2LQR才有唯一解。如果你修改了状态定义这个检查会让你少走很多弯路。另外还要注意A和B的维度匹配以及 Q 和 R 必须是对称半正定矩阵R 必须是正定矩阵。4.4 离散化、步长和实时性问题连续LQR求解得到的是连续控制器在Matlab离线仿真里直接使用没有问题因为仿真本身就是离散推进的。但如果你打算把代码移植到实车上控制器是跑在嵌入式板子上的数字控制程序建议改用离散LQR来设计。离散LQR可以用 dlqr 直接求解sys_d c2d(ss(A, B, [], []), dt); A_d sys_d.A; B_d sys_d.B; [Kd, Sd, ed] dlqr(A_d, B_d, Q, R);其中 c2d 是连续系统转离散系统的函数dt 是控制周期。对于本文的模型使用零阶保持器离散化后A_d 和 B_d 的数值与 A、B 略有差异但不会很大。关键点是控制周期越小连续和离散的差异越小但计算负载也越高。工程上常见控制周期是20到50毫秒本文的 dt 取0.05秒就属于这个区间。4.5 关于源码包中可扩展的设计方向最后说说这套源码怎么扩展成大一点的项目。我试验过的两个方向都挺实用。第一把 LQR 增益 K 设计成车速 v 的函数。由于 A 和 B 矩阵都包含 v车速一旦改变最优 K 就会变。你可以在离线圈里对每个速度档位预先计算 K然后生成查表也可以在每个控制周期根据当前 v 重新走一遍 lqr 求解。前者的实时性更好后者的实现更简单取决于你的算力余量。第二加入预瞄机制。LQR本身是纯反馈控制器它只对当前误差做出反应在曲率突变的工况下会有一定的滞后。预瞄控制会在参考轨迹上往前看一段距离用预瞄点代替最近点作为控制参考减少动态滞后。它本质上是给误差计算增加了一个偏移量代码改动不大但对双移线这类路径的动态性能提升非常明显。5. 最后再分享一点个人体会做了几轮车辆轨迹跟踪项目之后我最大的体会是LQR并不是一个花哨的算法但它把线性系统状态反馈的最优设计这件事讲透了。很多人觉得LQR参数不好调其实大部分情况是模型错误不是控制器问题。先把车辆模型、误差定义和坐标系拍清楚再谈Q和R基本就成功了一半。这套源码里所有代码都是用最朴素的方式写的没有花哨的语法封装每一行都能看懂改起来也不心虚。建议你先跑通圆形和直线两个最简单场景再切到双移线等把曲线跟踪的所有现象都看明白后再去折腾MPC和其他更复杂的方案也不迟。本文还有配套的精品资源点击获取
：清洗与实证应用指南)
