Matlab实现LQR车辆轨迹跟踪：从状态空间模型到完整源码

简介本资源是一份面向本科毕业设计与控制类课程作业的Matlab实践项目聚焦线性二次调节器LQR在车辆轨迹跟踪中的工程实现帮助学习者将控制理论转化为可运行的闭环控制系统。压缩包共9个文件包含4个.mat数据文件存储预设轨迹与误差数据、2个.m主程序脚本含主控逻辑与圆轨迹生成、2个.png仿真结果图直观展示跟踪效果及1份README.md说明文档整体仅107KB轻量易读、结构清晰。已有474人学习下载适合作为控制系统课程设计或毕业课题的参考范例。读者可直接运行main.m复现完整LQR控制器设计流程从车辆状态空间建模、Riccati方程求解、反馈增益矩阵K计算到实时轨迹跟踪仿真与横向误差分析代码注释详尽关键参数可调便于理解权重矩阵Q/R对跟踪性能的影响机制。 一直做路径规划相关的仿真最近被问得最多的就是“怎么让车沿着期望轨迹走而且别走偏”。用单纯的PID调参调到怀疑人生换MPC又觉得推导太复杂、参数太多。其实很多场景下LQR是最合适的折中方案尤其是用于简单的车辆轨迹跟踪既不需要复杂的非线性优化也不需要海量标定数据只要把状态空间模型和权重矩阵理清楚一套Matlab代码就能跑出很稳的跟踪效果。这篇内容围绕“Matlab实现LQR实现车辆轨迹跟踪完整源码”展开我会先解释为什么选LQR、怎么建立车辆误差状态模型再给出可复现的完整仿真代码最后分享调参经验和实际坑点。不管是正在做课程设计、毕业设计还是刚开始接触车辆横向控制的工程师都能在这篇文章里找到可以直接抄作业的思路。1. 轨迹跟踪问题拆解先搞清楚我们在解决什么1.1 路径跟踪和轨迹跟踪是两个完全不同的任务很多第一次做车辆控制的人会把“路径跟踪”和“轨迹跟踪”混为一谈但这两者的底层逻辑完全不一样。路径跟踪Path Following只要求车辆贴着参考几何路径走不关心何时到达某个点参考路径通常由弧长参数化车辆速度可以独立控制。轨迹跟踪Trajectory Tracking则要求车辆在特定的时间戳到达特定的空间位置参考轨迹不仅包含位置信息还包含期望速度、期望加速度。本文对应的“车辆轨迹跟踪”属于后者但在具体实现中如果只关注横向控制我们通常会把纵向速度固定转而把问题分解为“横向位置偏差和航向偏差的反馈镇定问题”。一旦速度恒定轨迹跟踪就退化为路径跟踪这让LQR这种线性控制器有了用武之地。还有一点特别容易踩坑LQR本质是线性系统的最优控制器但车辆运动模型本身是非线性的横向偏差、航向偏差、转向角之间存在三角函数关系。所以直接套用LQR几乎不可能稳定必须先做线性化处理。通常的做法是采用运动学自行车模型并在参考工作点附近进行小角度假设。1.2 为什么是LQR而不是PID或者MPCPID是很多人的第一反应毕竟实现简单、直觉清晰。但用PID做轨迹跟踪会遇到一个很尴尬的问题横向偏差和航向偏差存在强耦合关系单看某一个误差的PID很难同时兼顾曲线跟踪性能与稳定性。转弯的时候横向偏差PID会产生较大的转向角输入但航向PID同时在反向修正两个环容易互相干扰最终出现振荡。MPC虽然效果好能显式处理约束但需要构建预测时域上的优化问题。在Matlab里跑MPC仿真代码量通常比LQR多一个数量级而且每次要重新构造QP问题对刚入门的朋友并不友好。LQR正好处于两者之间它基于状态空间模型能同时考量横向偏差和航向偏差的多个状态变量借助Riccati方程直接求出最优反馈增益。这里的最优是指在给定Q、R权重下的线性二次型性能指标最小化它是在“性能最优”和“控制能量最小”之间做权衡。如果系统可控LQR增益唯一存在不需要像PID那样反复试凑。2. 车辆运动学模型与误差状态方程2.1 自行车模型的基本假设车辆在实际行驶中有复杂的横向动力学特性考虑轮胎侧偏、侧倾、悬架等但在低速轨迹跟踪场景下运动学自行车模型已经足够精准。它的核心假设是车辆左右轮的转向角相等把整车等效为前轮转向、后轮驱动的两轮自行车模型。模型的核心方程如下x_dot v * cos(psi) y_dot v * sin(psi) psi_dot v / L * tan(delta)其中x和y是后轴中心坐标psi是车辆航向角delta是前轮转向角L是轴距v是纵向速度。这个模型的状态量是[x, y, psi]控制量是[delta]如果车速可调则控制量还包括油门/制动。在后轴中心位置建立参考点时该模型具有一定的不完整性约束但用于LQR的设计并不会造成太大问题因为我们后面会构造误差状态模型而不是直接对x、y做控制。2.2 从误差角度建立LQR需要的状态空间方程轨迹跟踪的本质是让车辆的横向位置误差和航向误差逐渐收敛到零。我习惯把误差定义在“参考轨迹上离车辆最近的点”基准下e_y 横向位置误差车辆后轴中心到参考点的垂向距离 e_psi 航向误差车辆航向角减去参考点处的参考航向角如果车速恒定行驶路径曲率缓变那么误差状态满足如下近似的线性微分方程e_y_dot v * e_psi % 横向位置误差的变化率由航向误差和车速决定 e_psi_dot v / L * delta - psi_ref_dot其中psi_ref_dot就是参考点处的航向角变化率在参考轨迹已知的情况下可以直接计算psi_ref_dot v * kappa_refkappa_ref是参考路径曲率。写成矩阵形式就非常清晰了d/dt [e_y ] [0 v] [e_y ] [ 0 ] [ 0 ] [e_psi] [0 0] [e_psi] [ v/L ] delta [ -v * kappa_ref ]这里的A矩阵、B矩阵和扰动项非常直观。我们定义状态向量x_c [e_y; e_psi]控制量u delta那么标准状态空间表达式为x_c_dot A * x_c B * u w其中A [0, v; 0, 0] B [0; v/L] w [0; -v * kappa_ref]2.3 可控性与前馈补偿的必要性验证一下系统的可控性计算可控性矩阵Qc [B, A*B]。代入A和B可得B [0; v/L] A*B [v * v/L; 0]由于v和L都不为零Qc是满秩矩阵系统完全可控。这说明只要反馈增益选取得当我们一定能让横向误差和航向误差收敛。但注意扰动项w并非零直接使用反馈控制u -K*x_c无法完全消除参考曲率带来的稳态误差。因此实际工程中要在反馈控制的基础上加入前馈项。前馈补偿的物理含义很直接当车辆在曲率半径为R的弯道中匀速行驶时需要维持一个稳态转向角delta_feedforward L * kappa_ref。这是因为转向角的稳态值本来就由几何关系决定反馈控制只需要在这个基准上修正偏差。所以最终控制律为delta -K * x_c delta_feedforward这样的结构让LQR控制器在弯道中也不会出现明显的跟随滞后。很多只有纯反馈的LQR实现会在圆形轨迹跟踪时出现持续的横向偏差原因就是漏了前馈项。3. Matlab完整源码实现与逐步解析3.1 代码整体架构设计整套代码按功能分成几个文件便于阅读和调试main.m 主程序负责参数设置、仿真循环和可视化 create_reference.m 生成参考轨迹包括直线、圆、双移线等 bicycle_dynamics.m 自行车模型运动学方程 compute_reference.m 在参考轨迹上寻找最近点并计算参考航向角、曲率 lqr_solve.m 求解LQR增益矩阵K如果你是第一次跑不用一次性看完所有文件直接从main.m入口开始按顺序理解“生成参考轨迹 - 初始化车辆状态 - 求解LQR增益 - 进入仿真循环 - 绘制结果”这个流程。3.2 主程序整体流程与关键参数设置直接从main.m开始看%% 主程序LQR车辆轨迹跟踪仿真 clear; clc; close all; % 车辆参数 L 2.5; % 轴距 m v 5.0; % 恒定车速 m/s % 仿真参数 dt 0.05; % 采样时间 s T 30; % 仿真总时长 s N T / dt; % 总仿真步数 % LQR权重矩阵 Q diag([5, 2]); % 状态权重横向误差权重5航向误差权重2 R 0.5; % 控制量权重转向角 % 生成参考轨迹半径为15m的圆形轨迹 radius 15; cx 0; cy 0; ref_path create_reference(radius, cx, cy, v, dt, T); % 求解LQR增益 [K] lqr_solve(A, B, Q, R); % 注意这里的A、B需要先由车辆参数和车速构造 A [0, v; 0, 0]; B [0; v / L]; [K] lqr_solve(A, B, Q, R); % 初始车辆状态 x0 radius 2; % 在圆形轨迹外侧2m处起步 y0 0; psi0 pi / 2; % 初始航向角90度 state [x0; y0; psi0]; % 存储历史数据 x_hist zeros(1, N); y_hist zeros(1, N); psi_hist zeros(1, N); delta_hist zeros(1, N); ey_hist zeros(1, N); %% 仿真主循环 for k 1:N % 1. 在参考轨迹上寻找最近点 [ref_x, ref_y, ref_psi, ref_kappa, idx] ... compute_reference(ref_path, state(1), state(2), k); % 2. 计算横向误差和航向误差 % 横向误差车辆位置到参考点的横向偏差有符号 dx state(1) - ref_x; dy state(2) - ref_y; % 把位置偏差投影到参考航向的法线方向 cross_error -sin(ref_psi) * dx cos(ref_psi) * dy; % 航向误差标准化到[-pi, pi] e_psi wrapToPi(state(3) - ref_psi); % 3. 使用LQR反馈 前馈计算转向角 delta_fb -K(1) * cross_error - K(2) * e_psi; delta_ff L * ref_kappa; % 前馈项 delta delta_fb delta_ff; % 限制转向角范围 max_delta 30 * pi / 180; delta max(-max_delta, min(delta, max_delta)); % 4. 使用自行车模型更新车辆状态 state bicycle_dynamics(state, v, delta, dt, L); % 5. 保存历史数据 x_hist(k) state(1); y_hist(k) state(2); psi_hist(k) state(3); delta_hist(k) delta; ey_hist(k) cross_error; end这里有一个细节值得说一下LQR反馈计算时我直接写成了delta_fb -K(1)*cross_error - K(2)*e_psi。由于K是1x2的行向量所以更通用的写法是delta_fb -K * [cross_error; e_psi]效果是一样的。重点在于这里的状态是误差状态而不是车辆本身的x、y、psi状态。有人会误把车辆的x、y当作LQR输入状态那必然无法收敛。3.3 参考轨迹生成圆形轨迹与曲率的计算圆是最经典的LQR测试路径因为它的曲率恒定方便验证前馈补偿的效果。create_reference函数的实现如下function ref_path create_reference(radius, cx, cy, v, dt, T) % 生成圆形参考轨迹并以弧长为参数存储 % 返回结构体x, y, psi, kappa, s total_len v * T; s 0:dt*v:total_len; % 弧长序列 theta s / radius; % 圆心角序列 ref_path.x cx radius * cos(theta); ref_path.y cy radius * sin(theta); % 参考航向角圆的切向方向 ref_path.psi wrapToPi(theta pi/2); % 参考曲率1/radius ref_path.kappa ones(size(s)) / radius; ref_path.s s; ref_path.v v * ones(size(s)); end生成参考轨迹时要注意一个细节参考航向角psi的公式是theta pi/2这个推导很容易错。圆轨迹参数方程是x cx Rcos(theta)y cy Rsin(theta)对theta求导得到切向量[-Rsin(theta), Rcos(theta)]所以切线方向角是theta pi/2。如果你用arctan2(y_dot, x_dot)去算结果是一样的但要注意归一化处理。3.4 最近点搜索与参考信息的提取compute_reference是整套代码里最容易导致性能瓶颈的环节但也是保证跟踪精度的关键。它的任务是找到参考轨迹上离车辆当前位置最近的点并返回该点的航向角和曲率。function [ref_x, ref_y, ref_psi, ref_kappa, idx] ... compute_reference(ref_path, x_veh, y_veh, k) % 在参考轨迹上找到离车辆位置最近的点 % 为提高效率只搜索当前索引附近的局部区域 n length(ref_path.x); search_range 50; % 最多向前搜索50个点 % 在当前参考索引附近搜索 idx_start max(1, k - 5); idx_end min(n, k search_range); dist (ref_path.x(idx_start:idx_end) - x_veh).^2 ... (ref_path.y(idx_start:idx_end) - y_veh).^2; [~, loc] min(dist); idx idx_start loc - 1; ref_x ref_path.x(idx); ref_y ref_path.y(idx); ref_psi ref_path.psi(idx); ref_kappa ref_path.kappa(idx); end这种“局部搜索”的做法大幅提升了仿真速度因为轨迹跟踪场景下车辆不会偏离参考轨迹太远没必要从第1个点开始全轨迹搜索。但是要注意在仿真初期如果车辆初始偏差很大或者车辆与轨迹间存在较大的分离局部搜索可能找到的不是全局最近点。所以我把搜索起点设成k-5并允许向后索引一些点如果仍然担心鲁棒性可以在仿真前几十步使用全局搜索作为热启动。3.5 自行车模型更新四阶Runge-Kutta还是欧拉法在仿真精度要求不高的场景下使用简单的欧拉法就够了。但如果你希望看到更平滑的轨迹或者后续要把代码扩展到硬件在环仿真我会建议至少使用二阶Runge-Kutta中点法。这里给出一个简单而稳健的方法function state_new bicycle_dynamics(state, v, delta, dt, L) % 自行车模型运动学更新欧拉法 x state(1); y state(2); psi state(3); x_dot v * cos(psi); y_dot v * sin(psi); psi_dot v / L * tan(delta); state_new [x x_dot * dt; y y_dot * dt; psi psi_dot * dt]; state_new(3) wrapToPi(state_new(3)); end这里的wrapToPi很重要MATLAB内置的wrapToPi把角度限制在[-pi, pi]区间如果不做这个处理长时间仿真后航向角累积误差会导致cos和sin计算精度下降。这也是新手最容易忽视的细节。3.6 LQR求解不要手写Riccati方程有相当一部分教程会教你手写Riccati方程的迭代解法但在MATLAB中完全没必要。直接调用lqr函数即可function K lqr_solve(A, B, Q, R) % 求解LQR反馈增益矩阵K % 注意需要Control System Toolbox [K, ~, ~] lqr(A, B, Q, R); end这段代码简洁到让我自己都觉得“是不是太简单了”。但lqr函数确实做了所有核心工作验证矩阵维度、求解连续时间代数Riccati方程CARE、返回最优反馈增益。如果不想依赖Control System Toolbox也可以用care函数替代但既然装了Matlab直接用lqr就行了。3.7 可视化画出跟踪效果和误差变化没有可视化的控制仿真等于白做。绘制三张图就够用了%% 结果可视化 figure(Position, [100, 100, 1200, 400]); % 子图1轨迹跟踪对比 subplot(1, 3, 1); plot(ref_path.x, ref_path.y, k--, LineWidth, 1.5); hold on; plot(x_hist, y_hist, b-, LineWidth, 1.2); xlabel(x (m)); ylabel(y (m)); legend(参考轨迹, 实际轨迹); title(轨迹跟踪对比); grid on; axis equal; % 子图2横向误差变化 subplot(1, 3, 2); t_axis (1:N) * dt; plot(t_axis, ey_hist * 100, r-, LineWidth, 1.2); xlabel(时间 (s)); ylabel(横向误差 (cm)); title(横向误差变化); grid on; % 子图3转向角变化 subplot(1, 3, 3); plot(t_axis, delta_hist * 180 / pi, g-, LineWidth, 1.2); xlabel(时间 (s)); ylabel(转向角 (deg)); title(转向角变化); grid on;画完之后你会看到即使从初始横向偏差2米处起步LQR控制器也能在前几秒内把车辆拉回到参考轨迹附近稳态横向误差基本在厘米级甚至毫米级这就是前馈补偿的效果。4. 调参经验与常见问题排查4.1 权重矩阵Q和R的调参策略先别急着追求对称之美Q矩阵和R矩阵的选取直接决定LQR的控制行为这也是很多人卡住的地方。要知道LQR没有“绝对正确”的Q和R只有“符合需求”的Q和R。一个实用的调参策略是这样的先把R固定为1Q先设为对角线为1的矩阵跑一下仿真观察横向误差收敛速度和转向角大小。如果你希望横向误差收敛更快增大Q的第一行第一列横向误差权重。你会发现车“纠正偏差”的意愿更强但转向角波动也会变大。如果你希望转向更柔和、执行器负担更小增大R。转向角变化会明显变缓但误差收敛速度也会下降。关于航向误差权重Q(2,2)我会让它低于横向误差权重。因为航向误差的变化率本身与横向误差关联过高的航向权重会导致车辆频繁修正航向表现就是转向振荡。用一组经验参数参考车速5m/s、轴距2.5m的模型中Q diag([5, 1])、R 0.3已经是比较均衡的起点。具体怎么调还是要配合仿真图来看。调参的过程比较枯燥但相信我经过几次“Q加大导致抖动、R加大导致跟踪变慢”的反复之后你会对LQR的控制逻辑有更直观的理解。4.2 为什么我的LQR在弯道中总有一圈稳态误差这是最常见的问题几乎每隔几天就会有人问一次。如果你只用了反馈控制u -Kx_c在任何曲率不为零的路径上都会存在稳态误差。原因就是前面提到的扰动项w [0; -vkappa_ref]没有被抵消。通俗点说LQR反馈只负责“纠偏”但车辆在弯道上本身就需要的那个基础转向角反馈控制是“管不着”的。如果没有前馈补偿控制器需要积累一个恒定的航向误差才能让车轮产生足够的转向角于是车辆就会一直在弯道内侧“压着”轨迹走表现为持续的横向偏差。解决方法就是加上delta_ff L * ref_kappa的前馈补偿。加了之后车辆在进入弯道前就能提前打好方向反馈只需要处理模型误差和外界扰动。我在调试圆形轨迹时对比过不加前馈时稳态横向误差大约20cm加上前馈后直接降到1cm以内。4.3 转向角饱和控制器能算出超过物理极限的转角Matlab仿真中一切输入输出都是理想化的但任何真实车辆都有转向角限制。一般家用车的方向盘转角换算到前轮转角大概在±30度到±35度如果你在LQR设计中完全不管这个限制控制器可能会计算出一个完全无法执行的大转角仿真结果看似正常但实际应用时车辆会失控。解决方式是在控制律之后加一个饱和函数代码中我已经写了max_delta 30 * pi / 180; delta max(-max_delta, min(delta, max_delta));加饱和函数之后要注意一个问题如果车辆初始偏差非常大LQR会输出饱和的转向角但仍然不够快地把误差拉回这导致实际响应比无饱和场景差很多。这种情况可以适当增加Q中横向误差的权重让控制器“更激进”地纠偏而饱和限幅本身也能保证转角不超出物理限制。饱和决策应该在仿真中尽早验证否则做硬件时容易心态崩。4.4 为什么车速改变后LQR增益需要重新求解很多人会认为LQR增益矩阵K只要解一次就够了但事实上系统矩阵A和B中包含车速v车速变化后整个系统模型就变了。从A [0, v; 0, 0]B [0; v/L]可以看出v从5m/s变成10m/s系统的动态响应特性完全不同同一个K在高速时可能稳定低速时却可能出现振荡。这就是变增益LQRGain Scheduling的出发点。标准的做法是在不同速度区间分别计算对应的K并做查表插值。但如果你只想在固定速度下做轨迹跟踪那直接在初始化阶段把K算好就可以了。本文代码设定的v 5m/s如果改变v记得重新运行lqr求解。4.5 初始航向偏差特别大时为什么跟踪出现了“绕圈”现象当车辆初始航向与参考航向相差接近180度时LQR状态模型中的小角度假设已经失效。我的自行车模型运动学方程本身是非线性的但误差模型是线性的这要求航向误差足够小LQR才有效。如果初始航向误差达到90度以上车辆可能先去绕一个圈再慢慢纠正回来。解决思路有两个在启动LQR之前增加一个“方向调整阶段”比如先用一个简单的纯追踪算法把航向大致对齐再切换到LQR。使用非线性控制比如后推法或滑模控制但这会让代码复杂度大幅上升。更简单的工程妥协修改初始状态让仿真从一个更合理的初始条件开始比如初始航向误差小于20度LQR的表现就能很好。对于绝大多数轨迹跟踪场景车辆不会在极端姿态下开始控制所以这个问题在使用时注意一下即可。4.6 采样时间dt如何选择dt的选择直接影响仿真的稳定性和真实性。dt过大欧拉法离散化误差会累积甚至导致系统不稳定dt过小仿真的计算量增大但没有实际的精度收益。对于运动学自行车模型车速5m/s轴距2.5mdt选择0.05s已经能满足大部分仿真需求。如果你要扩展到真实控制器考虑一下控制器的运行频率一般底盘控制的周期在10到50ms之间dt取0.01s到0.05s是比较合理的范围。需要对比测试的话可以写一个dt参数的循环在相同条件下跑多个仿真观察横向误差的收敛轨迹选择误差不再显著减小的那个dt作为临界值。4.7 完整源码中可能遇到的Matlab版本兼容问题我在不同的Matlab版本上跑过这份代码主要兼容性差异集中在两个点一是lqr函数需要Control System Toolbox。如果用的是MATLAB Online基本版或者没有安装工具箱lqr会直接报错。建议在脚本开头加一句环境检查if ~license(test, Control_Toolbox) error(需要安装 Control System Toolbox 才能运行LQR求解); end二是wrapToPi函数属于Mapping Toolbox。虽然大部分发行版都内置但少数基础版可能没有。如果遇到这个函数未定义可以自己实现function psi_norm wrapToPi(psi) psi_norm atan2(sin(psi), cos(psi)); end这个自实现代码不受工具箱限制而且因为这里三角函数计算比较稳定放在常规版本下也表现得很好。5. 进阶扩展从简单圆形到复杂轨迹5.1 双移线轨迹更接近真实驾驶场景的测试圆轨迹虽然便于验证基础原理但说实话太“理想”了。真实道路中车辆经历的更多是渐变曲率、直线和曲线交替的复杂路段。双移线Double Lane Change是车辆横向控制测试中的经典场景它模拟的是高速行驶中变道避障再回到原车道的动作。如果要把参考轨迹换成双移线核心改动在create_reference函数。双移线的参考Y坐标表达式可以写成% 双移线参考轨迹 function ref_path create_double_lane_change(v, dt, T) total_len v * T; s 0:dt*v:total_len; % 双移线横向偏移使用斜坡加平滑过渡 lane_offset 3.5; % 车道宽度 X s; Y lane_offset / 2 * (1 tanh(0.2 * (X - 20))) - ... lane_offset / 2 * (1 tanh(0.2 * (X - 60))); ref_path.x X; ref_path.y Y; % 参考航向角由路径导数计算 dYdX 0.2 * lane_offset / 2 * (1 - tanh(0.2 * (X - 20)).^2) - ... 0.2 * lane_offset / 2 * (1 - tanh(0.2 * (X - 60)).^2); ref_path.psi atan2(dYdX, 1); % 参考曲率由二阶导计算 d2YdX2 -0.04 * lane_offset * tanh(0.2 * (X - 20)) .* (1 - tanh(0.2 * (X - 20)).^2) ... 0.04 * lane_offset * tanh(0.2 * (X - 60)) .* (1 - tanh(0.2 * (X - 60)).^2); ref_path.kappa abs(d2YdX2) ./ (1 dYdX.^2).^(3/2); end这里用tanh函数生成平滑的过渡曲线最大的好处是曲率连续不会对转向角产生阶跃冲击。曲率计算直接用二阶导公式虽然比数值微分方法略复杂但精度更高前馈补偿的稳定性也更好。5.2 车速变化时的处理思路本文代码中车速固定为零5m/s但真实场景中车辆在弯道中通常要减速。如果你希望加入纵向速度控制LQR设计就需要修改为纵向控制器与横向控制器协同工作。一个简单的思路是根据参考轨迹的曲率预先计算期望速度例如v_ref min(v_max, sqrt(a_lat_max / abs(kappa)))其中a_lat_max是最大横向加速度。纵向控制使用一个简单的PID控制器跟踪期望速度。横向LQR的增益需要根据实时速度动态插值这也是前面提到的变增益LQRGain Scheduling思想。这样的完整系统虽然代码量更大但配合本文的动态LQR增益计算在架构上是非常自然的扩展。5.3 离散化LQR从连续到离散的跨越代码中使用的lqr函数求解的是连续时间系统的Riccati方程。但在实际工程特别是嵌入式控制器中系统的采样是离散的更严谨的做法是使用离散LQR即dlqr函数。离散LQR的问题定义是最小化J sum(xQx uRu)而不是积分形式。如果你的控制器运行频率很高比如100Hz以上连续LQR与离散LQR之间的增益差异可以忽略不计。但如果运行频率较低例如10到20Hz连续LQR算出来的增益可能导致系统不稳定此时必须用dlqr。Matlab中使用dlqr也非常简单% 将连续系统离散化 sys_c ss(A, B, eye(2), 0); sys_d c2d(sys_c, dt, zoh); Ad sys_d.A; Bd sys_d.B; [Kd, ~, ~] dlqr(Ad, Bd, Q, R);这里的c2d使用零阶保持器ZOH它假设控制量在一个采样周期内保持不变这正好符合数字控制器的实际行为。对采样频率较高的系统连续和离散的区别不大但理解这一点能帮你避免不少硬件调试时的低级问题。6. 实操心得与工程落地建议这套LQR轨迹跟踪代码我在课程设计、毕业设计辅助和几个小型原型车上都反复用过。说几个感触最深的地方第一别迷信单一指标的最优性。LQR的“最优”是在Q、R定义下性能指标最小化但指标本身是你定的如果Q、R的工程含义没有想清楚得到的结果很可能“最优但不好用”。有一个做法是用仿真结果逆推Q、R的实际意义。比如横向误差从2米收敛到0.1米花了5秒你觉得太慢就找横向误差权重的贡献到底体现在哪一步而不是盲目把Q提高100倍。第二我强烈建议在改任何参数之前先把所有历史数据保存下来。调参最容易犯的错误是“同时改了好几个参数”然后发现某个现象变好了却不知道是哪个改动起了作用。正确的做法是保持其他不变单次只改动一个参数记录仿真曲线再做对比。第三这份代码里最容易被忽略但收益最高的是前馈补偿。如果你只想要一个“能跑”的轨迹跟踪纯反馈LQR也能跑弯道会有一点偏差但不会发散。不过只要加上一行前馈效果立刻脱胎换骨。这个经验不只是LQR在MPC、Stanley等其他控制器里也一样成立要充分利用参考轨迹提供的曲率信息。第四如果要做实物验证建议先用Simulink搭建一个完整的整车模型做硬件在环测试不要一上来就把这份Matlab脚本部署到嵌入式控制器里。因为脚本中的最近点搜索、角度归一化、饱和约束等逻辑在硬件里需要重新设计成实时可执行的代码这中间涉及到的工程细节比控制器本身还多。最后分享一个小技巧如果想让车辆启动时更平稳可以在仿真前1秒内对LQR的输出做一个线性斜坡限幅让转向角从零逐渐过渡到LQR算出的值。这个处理在纯仿真里看不到什么区别但在实际车辆上能明显减少启动时的冲击感。代码实现非常简单在控制律之后加一个限幅斜率即可很多底盘控制器本身也支持。这份源码从模型建立、误差推导、LQR求解到仿真验证已经覆盖了车辆轨迹跟踪最核心的闭环逻辑。你拿到手之后先原样跑通一遍然后动手改Q、R权重看效果再尝试换参考轨迹最后考虑添加速度控制。走完这几步LQR轨迹跟踪控制就算真正掌握了。本文还有配套的精品资源点击获取