LQR车辆轨迹跟踪Matlab实现：从运动学建模到调参实战

简介：本资源是一份面向本科毕业设计与控制类课程作业的Matlab实践项目，聚焦线性二次调节器（LQR）在车辆轨迹跟踪中的完整实现，帮助学习者将控制理论转化为可运行、可验证的闭环控制系统。压缩包共9个文件，含4个.mat数据文件（存储预设路径、横向误差等关键轨迹与状态数据）、2个.m脚本（主程序main.m与圆形路径生成函数CircleGen.m）、2个.png结果图（直观展示跟踪效果）及1份README.md说明文档，整体仅107KB，轻量易读、结构清晰。已有474人学习下载，适合作为控制工程入门到进阶的典型范例。读者可直接运行复现LQR控制器设计全流程：从车辆线性化状态空间建模、Q/R权重调参、Riccati方程求解，到反馈增益K计算与实时轨迹跟踪仿真，代码注释详尽，变量命名规范，便于理解算法逻辑与调试思路。 如果你也在做车辆横向控制，或者打算用Matlab跑一段轨迹跟踪demo，LQR（线性二次型调节器）应该是跳不过去的一个方法。我最近整理了一套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) dt

Q 是状态权重矩阵，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 和航向误差 e2：

e1 = -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)；如果转角抖得厉害，优先调大 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.m

main.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

关于 wrapToPi，Matlab的 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做了一个对比实验，数据趋势非常典型：

这个表印证了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，控制量维度是1，rank(Co)必须等于2，LQR才有唯一解。如果你修改了状态定义，这个检查会让你少走很多弯路。另外还要注意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和其他更复杂的方案也不迟。

本文还有配套的精品资源，点击获取