简介一套运用线性二次型调节器LQR实现车辆轨迹跟踪的Matlab完整方案面向计算机、自动化及相关专业学生的课程设计、期末大作业与实践训练。方案经导师指导并获98分评定覆盖状态空间建模、权重矩阵配置、闭环系统稳定性分析等核心技术环节帮助读者建立从理论到仿真的完整认知。压缩包共13个文件以mat数据文件、m源码、md说明文档为主辅以png示意图及备份zip包整体约176KB结构清晰便于查阅。资源附有主程序、圆轨迹生成器、误差数据与配套说明可完整复现车辆跟踪控制流程也可为同类课题提供直接参考。已有38人学习使用适合希望深入理解最优控制在车辆工程中应用的学习者。1. LQR车辆轨迹跟踪到底在解决什么问题从直线稳到弯道跟上做车辆轨迹跟踪的人多半先试过PID转向控制直线循迹还不错一进弯道就出现外侧漂移曲率再大一点就开始画龙。LQR控制器解决的就是这个问题——它把「横向偏差」和「航向偏差」同时放进一个二次代价函数通过状态反馈一次算出最优前轮转角跟踪直线和弯道用的是同一套逻辑。这套Matlab实现方案面向三类人做智能车循迹课程设计的同学、要把控制算法部署到实车控制器上的工程师、以及想弄懂MPC之前先把线性最优控制吃透的开发者。不需要昂贵的仿真软件一台装Matlab的普通电脑就能把方案跑通核心代码量不超过一百行。2. 从运动学模型到LQR设计误差状态方程、可控性与离散化LQR是模型控制算法模型选错后面所有调参都白费。车辆轨迹跟踪的LQR方案里最常见也最好用的模型是运动学自行车模型误差状态方程则是在这个模型基础上推导出来的。这一章把建模到离散化的每一步拆开。2.1 选哪个模型运动学自行车模型与它的适用边界车辆在平面上运动如果不考虑轮胎侧偏可以把整车简化为「前轮转向、后轮驱动」的自行车模型。以后轴中心为参考点轴距记为L前轮转角为delta航向角为psi纵向车速为v运动学方程为dx/dt v*cos(psi)dy/dt v*sin(psi)dpsi/dt v*tan(delta)/L这个模型只有轴距一个物理参数标定成本极低在中低速场景下精度足够。适用边界要记住车速建议控制在15m/s以内约54km/h侧向加速度不要超过0.5g。超过这个范围轮胎侧偏不可忽略运动学模型预测的横摆角速度会偏大控制器给的前轮转角也会偏大。% 车辆运动学模型参数SI单位 L 2.5; % 轴距 2.5 m v 5; % 纵向车速 5 m/s约18 km/h dt 0.05; % 控制周期 50 ms这里先把车速v固定为常量原因是LQR的系统矩阵A和B里都含有vv恒定时增益K才是一个常数矩阵。后面如果要处理加减速工况需要引入增益调度这个留到最后一章展开。控制周期50ms对应20Hz的控制频率是实车底盘控制里比较常规的刷新率。2.2 误差状态方程怎么推横向误差和航向误差的动态LQR需要的是线性状态方程车辆运动学模型本身是非线性的所以要在「误差」上做线性化。定义两个状态e_y是车辆位置相对于参考轨迹投影点的横向偏差左侧为正e_psi是车辆航向角与参考航向角的差值。假设参考轨迹上投影点处的曲率为kappa_ref参考航向角为psi_ref经典Frenet坐标系下的误差动态忽略投影点纵向速度与车速的微小差异为de_y/dt vsin(e_psi) ≈ ve_pside_psi/dt vtan(delta)/L - vkappa_ref ≈ vdelta/L - vkappa_ref小角度近似成立的前提是e_psi在±0.3rad以内tan(delta)在delta不超过0.4rad时与delta的误差约5%。车辆循迹过程中航向偏差一般远小于这个范围近似是合理的。写成标准状态空间形式d/dt [e_y; e_psi] [0, v; 0, 0] * [e_y; e_psi] [0; v/L] * delta [0; -v*kappa_ref]其中最后一项是参考轨迹曲率带来的已知扰动控制器里用前馈抵消它。可控性矩阵rank([B, AB])对这套系统是2完全可控意味着LQR可以任意配置闭环极点不存在「控制不了某个方向」的问题。2.3 连续设计还是离散设计c2d dlqr 是标准做法Matlab里做LQR有两条路对连续系统直接调lqr()或者先把连续系统离散化再用dlqr()。我一般用后者原因很实际控制算法最终跑在数字控制器上零阶保持器假设控制信号在每个周期内保持不变c2d(, zoh)恰好模拟了这种行为拿到的离散增益Kd直接就能落代码。% 连续状态矩阵车速 v 5 m/s A [0, v; 0, 0]; B [0; v/L]; C eye(2); % 两个状态直接可测不需要观测器 D zeros(2, 1); % 连续 LQR 设计用于对比 Q diag([10, 1]); R_ctrl 1; [Kc, S, ~] lqr(A, B, Q, R_ctrl); % 零阶保持离散化控制周期 0.05 s sys_c ss(A, B, C, D); sys_d c2d(sys_c, dt, zoh); [Kd, ~, ~] dlqr(sys_d.A, sys_d.B, Q, R_ctrl);逻辑说明c2d做的是矩阵指数积分离散化比朴素的欧拉近似A_d I Adt, B_d Bdt保留了更多连续系统动态在dt取到0.1s时优势非常明显。dlqr的Q和R与连续lqr里的物理意义一致权重数值可以直接沿用。参数说明Q是2x2对角阵第一个对角元对应横向误差权重第二个对应航向误差权重R_ctrl是控制量权重。dt改变时sys_d.A和sys_d.B跟着变Kd也要重新算所以仿真步长和控制周期必须对齐这一点后面避坑章节还会专门讲。2.4 前馈项让稳态误差归零跟踪曲线轨迹的关键补丁只靠状态反馈跟踪恒定曲率弯道会留下一个稳态误差。原因很直观误差动态方程里有一项-v*kappa_ref它是个常值扰动反馈控制要永久地输出一个转角去抵消它误差就不可能归零。解决办法是在控制量里叠加前馈转角。参考曲率对应的稳态转向角是delta_ff atan(L * kappa_ref)把它加进去后误差动态变成齐次方程反馈项的职责不再是抵消扰动而是纯粹把初始误差和随机扰动收敛到零。% 前馈转角参考曲率对应的稳态转向角 delta_ff atan(L * kappa_ref(idx)); % 反馈转角 delta_fb -Kd * [e_y; e_psi]; % 总转角 delta delta_fb delta_ff;注意前馈用的是atan(Lkappa_ref)而不是直接Lkappa_ref。曲率较小时两者几乎相等但在半径5m的急弯上L*kappa达到0.5tan和atan的差距已经超过10%直接线性近似会让前馈转角偏小稳态横向偏差又冒出来。3. Matlab实现LQR轨迹跟踪参考轨迹、误差匹配与主循环代码这一章把整个方案跑起来。从生成参考轨迹开始到误差计算再到主循环里的控制量求解和状态更新每段代码都可以直接复制进Matlab运行。3.1 参考轨迹生成圆形轨迹的坐标、航向和曲率验证LQR方案最常用的轨迹是圆曲率恒定方便后面分析稳态误差。半径取10m采样1000个点直接用参数方程生成x_ref、y_ref再算出每个点的航向角和曲率。% 圆形参考轨迹半径 10 m R_c 10; samples 1000; s_arc linspace(0, 2*pi*R_c, samples); % 弧长参数0~62.83 m x_ref R_c * cos(s_arc / R_c); y_ref R_c * sin(s_arc / R_c); % 切线航向角 圆心极角 pi/2再归一到 (-pi, pi] psi_ref wrapToPi(s_arc / R_c pi/2); kappa_ref (1 / R_c) * ones(samples, 1); % 圆的曲率恒定 % 画图检查轨迹形状 plot(x_ref, y_ref, LineWidth, 1.5); axis equal; grid on; xlabel(x (m)); ylabel(y (m));逻辑说明圆的切线方向是极角方向加90度wrapToPi把它归一化到(-pi, pi]区间后续计算航向误差时不会出现2pi的跳变。kappa_ref直接取半径倒数这个环节如果出错跟踪肯定跑不好。参数说明samples决定参考点密度1000个点对应相邻点间隔约6cm。在5m/s车速下每个控制周期走0.25m大约跨过4个参考点密度足够。如果点太稀最近点匹配的量化误差会把控制量弄抖这个后面也会遇到。3.2 最近点匹配与误差投影从全局搜索到窗口搜索要计算横向误差第一步是找到车辆当前位置在参考轨迹上的投影点。最朴素的做法是全局最近点搜索但急弯处容易出现匹配点来回跳的问题所以工程上更常用的是带窗口限制的搜索。先看全局版本再升级成窗口版本。% 全局最近点匹配低速、曲率小的轨迹可用 d2 (x_ref - xk).^2 (y_ref - yk).^2; [~, idx] min(d2); % 窗口搜索限制在上次匹配点附近 search_lo max(1, idx_prev - 20); search_hi min(samples, idx_prev 80); d2_win (x_ref(search_lo:search_hi) - xk).^2 ... (y_ref(search_lo:search_hi) - yk).^2; [~, offset] min(d2_win); idx search_lo offset - 1;逻辑说明轨迹是连续的车辆一个周期内最多前进零点几米匹配点不可能在索引空间里跳很远。窗口搜索把范围限制在上次匹配点附近后那些「跨弧段跳变」的毛刺就消失了。向后留80个点是因为车辆往前跑匹配点只会向索引增大的方向移动向前留20个点是允许在曲率半径很小的弯道上投影点相对上一周期短暂回退。参数说明窗口宽度要大于车辆一个控制周期内的位移量。5m/s、0.05s周期下位移0.25m约4个参考点向前20个点的余量已经非常充分。找到匹配点后把车辆当前位置投影到参考坐标系里求横向误差% 投影到参考点切向/法向坐标系左正右负 dx xk - x_ref(idx); dy yk - y_ref(idx); e_y -sin(psi_ref(idx)) * dx cos(psi_ref(idx)) * dy; % 航向误差必须做角度归一化 e_psi psi - psi_ref(idx); e_psi wrapToPi(e_psi);逻辑说明参考点处建立一个切向和法向坐标系横向误差就是车辆位置在法向方向的分量。公式里选法向左侧为正和e_psi大于0表示车头偏左的定义保持一致这样状态反馈的符号才不会反。3.3 主循环完整代码LQR控制、限幅与车辆状态更新把前面的模块串起来就是完整的仿真主循环。车辆初始位姿故意和参考轨迹错开一段距离用来观察LQR的收敛过程。% 初始状态车辆在原点航向0°参考圆圆心在(0,10) xk 0; yk 0; psi 0; idx_prev 1; % LQR 增益来自上一章的 KdKd 是 1x2 行向量 max_steer 0.4; % 前轮转角限幅约23° N 4000; % 仿真步数4000*0.05 200 s % 记录数组 e_y_record zeros(N, 1); delta_record zeros(N, 1); for k 1:N % 1. 参考点匹配窗口搜索 search_lo max(1, idx_prev - 20); search_hi min(samples, idx_prev 80); d2_win (x_ref(search_lo:search_hi) - xk).^2 ... (y_ref(search_lo:search_hi) - yk).^2; [~, offset] min(d2_win); idx search_lo offset - 1; idx_prev idx; % 2. 横向误差与航向误差 dx xk - x_ref(idx); dy yk - y_ref(idx); e_y -sin(psi_ref(idx)) * dx cos(psi_ref(idx)) * dy; e_psi wrapToPi(psi - psi_ref(idx)); % 3. LQR 反馈 前馈 delta_fb -Kd * [e_y; e_psi]; delta_ff atan(L * kappa_ref(idx)); delta delta_fb delta_ff; % 4. 前轮转角限幅 delta max(-max_steer, min(max_steer, delta)); % 5. 运动学模型更新 psi psi v * tan(delta) / L * dt; xk xk v * cos(psi) * dt; yk yk v * sin(psi) * dt; % 记录 e_y_record(k) e_y; delta_record(k) delta; end % 运行后检查 figure; subplot(2,1,1); plot((1:N)*dt, e_y_record); ylabel(e_y (m)); grid on; subplot(2,1,2); plot((1:N)*dt, delta_record); ylabel(delta (rad)); grid on;逻辑说明主循环五步的顺序不能乱——先匹配参考点再算误差然后算控制量限幅之后才去更新车辆状态。记录数组预分配好避免循环里动态扩展数组拖慢速度。运行后眼睛先看两条曲线e_y能不能收敛到0附近delta有没有长时间贴在限幅值上。参数说明Kd是dlqr返回的行向量和列向量误差相乘得到标量反馈转角max_steer取0.4rad是多数乘用车前轮转角的物理上限实车要根据转向传动比换算。车速v在循环里保持不变如果你试着自己改成时变车速要记得psi更新式里的v换成当前周期的值Kd也不能再用常量。4. Q和R矩阵的调参规律尺度校正、调参顺序与效果对比LQR调参最容易变成「玄学」因为Q和R的数值本身没有绝对意义只有相对意义。这一章把两个矩阵的物理作用讲透再给一套不用瞎猜的流程。4.1 Q和R各压制什么横向误差、航向波动和控制量代价函数J ∫(x^T Q x u^T R u)dt里Q对角线上的两个元素分别惩罚横向误差和航向误差R惩罚控制量幅度。Q里对应元素调大闭环增益变大误差收敛更快但代价是控制量更猛R调大控制量被压住转角更平滑误差收敛变慢。具体趋势用下面这张表概括。参数调整横向误差峰值控制量波动收敛速度典型场景Q(1,1) 增大明显减小增大加快跟踪精度优先Q(2,2) 增大略有减小启动瞬间更猛航向修正加快初始航向偏差大R 增大略有增大明显减小变慢执行器平滑优先这张表是趋势不是精确数值预测。我在调参时先把Q(2,2)和R固定单独动Q(1,1)看横向误差峰值的变化这样一组实验做完Q(1,1)的作用边界就摸清了。4.2 尺度校正1米和1弧度不是同一个数量级新手最容易犯的错是设Q diag([1, 1])。表面上看两个状态权重相等实际上完全不是。横向误差0.2m时代价是0.04航向偏差0.05rad时代价只有0.0025两者差了一个数量级。控制器会拼命压横向误差但航向偏差几乎不受惩罚表现就是车辆「斜着走」也不修正航向。经验做法是让Q(1,1)和Q(2,2)的比例落在10到100之间。原因是典型工况下e_y的量级是0.1me_psi的量级是0.01到0.05rad要让两个状态在代价函数里的贡献可比权重比就该反比于量级比。从这个角度看Q diag([10, 1])是个非常合理的起点。R的数值同样要放到量级里看。R_ctrl 1意味着控制量delta的平方直接进代价函数而delta的正常工作范围是0.1到0.5rad平方后是0.01到0.25和误差项的代价在可比范围内。R如果取100转角稍微动一下就产生巨大的代价控制器会变成「不敢转」的状态。4.3 一套可复现的调参顺序与效果对比我一般按下面这个顺序走每一步都能量化固定R_ctrl 1Q(2,2) 1Q(1,1)分别取1、10、100、1000跑四轮仿真记录峰值横向误差和控制量峰值找到横向误差达标且控制量还能接受的Q(1,1)最后调R压掉控制量的高频抖动批量对比用脚本跑不用手动改参数。q11_list [1, 10, 100, 1000]; R_fixed 1; peak_e_y zeros(size(q11_list)); peak_delta zeros(size(q11_list)); for i 1:length(q11_list) Q_i diag([q11_list(i), 1]); [Kd_i, ~, ~] dlqr(sys_d.A, sys_d.B, Q_i, R_fixed); % 用第3章主循环把Kd替换成Kd_i跑完后取统计量 % 主循环内部代码这里省略直接记录结果 peak_e_y(i) max(abs(e_y_record)); peak_delta(i) max(abs(delta_record)); end % 打印对比表 disp(table(q11_list, peak_e_y, peak_delta, ... VariableNames, {q11, peak_e_y, peak_delta}));逻辑说明这样能把「权重增加10倍」和「横向误差峰值下降多少」直接对应起来调参从拍脑袋变成查表。注意每次循环里Kd_i必须重新用dlqr计算不能复用上一轮的增益。参数说明q11_list用对数间隔10倍步进能在4轮内覆盖三个数量级很快找到敏感区间。如果峰值误差从q1110到q11100下降不明显说明误差瓶颈不在横向权重上去查前馈或者参考点密度。以半径10m的圆、车速5m/s、初始偏差(2, -1)为例我自己的调参记录大致是Qdiag([1,1])时峰值横向误差0.5m转向角峰值0.3rad轨迹上有可见摆振Qdiag([10,1])时峰值误差0.12m转角0.15rad曲线平滑Qdiag([100,1])时峰值误差0.05m但启动瞬间转向角速度太快执行器有压力最后选了Qdiag([50,1])、R1峰值误差0.06m转向峰值0.17rad兼顾精度和执行器负担。调参不是权重越大越好控制量抖起来之后反而影响跟踪。5. LQR轨迹跟踪高频问题与排查5个避坑记录这套方案我前后在多个模拟项目里跑过也在某跨平台系统的样车上见过真车表现。这里把高频踩坑按「现象→原因→解决」写清楚照着排查能省大量时间。5.1 航向误差跳变wrapToPi省掉之后控制量突变现象跟踪大圆轨迹时车辆经过某个位置后前轮转角猛地反向打到最大轨迹出现一个尖刺随后又恢复正常。原因psi_ref经过wrapToPi归一化后从接近pi的值直接跳到接近-pi的值。如果求差值时不归一化两个实际相差不到0.1rad的角度会被算成相差约6.2radLQR把它当成巨大航向误差输出一个巨大的反向转角。解决e_psi wrapToPi(psi - psi_ref(idx))这是必须项不是可选项。排查方法也简单画e_psi的时序图正常范围应该在±pi内波动如果看到一个突发的阶跃跳变就是这里漏了。5.2 最近点毛刺急弯处参考点来回跳现象曲率半径小于5m的弯道上e_y波形出现锯齿状抖动但参考轨迹本身是光滑的。原因全局最近点搜索在弯道内侧和外侧之间来回切换不同索引处psi_ref差异很大匹配点一跳误差投影也跟着跳。车辆明明在平稳行驶控制量却被「想象出来的误差」带着抖。解决改用窗口搜索第3章代码以上一周期匹配点为中心限制搜索范围。注意向前搜索的窗口长度要覆盖车辆一个控制周期内的位移否则匹配点跟不上车辆。如果窗口改完还有毛刺把窗口上限缩到40个点试试。5.3 大步长仿真发散连续增益拿到离散系统里直接用现象dt改成0.1s后仿真不到几秒就发散dt0.01s时一切正常让人误以为是参数问题。原因连续系统设计的LQR增益直接用在离散系统里相当于在一个存在零阶保持延迟的被控对象上施加了一个「按连续域相位裕度设计」的控制器。零阶保持器引入的相位滞后随dt增大而增大大步长下把连续域设计留下的稳定裕度消耗殆尽。dt0.01s时滞后很小问题被掩盖dt0.1s时滞后显著闭环开始振荡。解决统一走c2d dlqr流程离散化后的Kd直接可部署。如果坚持用连续增益控制周期至少要压到0.02s以下且要把R适当调大压低增益。5.4 高速场景模型失配运动学模型的边界在哪现象车速加到15m/s以上用同一个仿真模型调好的参数换到高保真车辆动力学测试环境里横向误差明显变大高速急弯处甚至发散。原因运动学模型假设轮胎无侧偏横摆角速度等于v*tan(delta)/L。真实车辆在高速转向时轮胎侧偏让实际横摆角速度小于这个值。控制器按照模型计算出「刚好」的转角实际车辆却转过不够横向误差持续累积反馈再加大转角最终进入恶性循环。解决这个方案的适用边界就是中低速v 15m/s侧向加速度不超过0.5g。超出边界换成带轮胎侧偏刚度的动力学误差模型状态量里加入侧偏角和横摆角速度。如果只能在这个方案上做修补限幅需要随车速收缩、R随车速增大让高速下控制作用整体变保守。5.5 限幅后误差不收敛控制饱和的识别现象delta_record显示输出长期贴在0.4rad上限e_y不再下降甚至缓慢增大。原因输入限幅和LQR请求的控制量方向一致但幅度被截断系统进入深度饱和。此时反馈早就不在线性区工作继续调Q、R都不会有效果。解决先确认饱和。看delta_record时序连续20步以上贴在限幅值边缘基本就是深度饱和。应对办法按优先级第一检查前馈与反馈方向是否一致频率不高但偶发出现两者方向相反的情况第二增大R让LQR在设计阶段就把控制量压到限幅以内第三降低车速或者改用曲率更小的参考轨迹从源头上减小控制需求。可以在主循环里加一个简单的警告逻辑。% 连续 20 步贴住限幅则告警 if abs(delta_record(k)) max_steer * 0.98 sat_count sat_count 1; else sat_count 0; end if sat_count 20 warning(控制量长时间饱和请增大R或降低车速); end逻辑说明限幅是物理约束LQR在线性域内的最优解在饱和区没有意义。这个警告逻辑能第一时间把问题揪出来而不是等看到e_y发散才回头查。6. 再进一步增益调度、时变车速与验证方法方案跑通之后往实用方向走还有三步车速变化时的增益调度、方案验证方法、以及部署到控制器之前要确认的事。6.1 车速变化时的增益调度前面全程假设v是常量但实际车辆加减速时v一直在变。A和B矩阵里含v严格来说每个车速对应一组不同的K。2x2系统的LQR求解代价极小离线算好一组车速-增益映射表运行中插值取用即可。v_grid 2:1:20; K_map cell(length(v_grid), 1); for i 1:length(v_grid) A_v [0, v_grid(i); 0, 0]; B_v [0; v_grid(i)/L]; sys_d_v c2d(ss(A_v, B_v, eye(2), 0), dt, zoh); [K_map{i}, ~, ~] dlqr(sys_d_v.A, sys_d_v.B, Q, R_ctrl); end % 运行中按当前车速插值 v_now 6.3; low floor(v_now); high low 1; alpha (v_now - v_grid(low)) / (v_grid(high) - v_grid(low)); Kd_now (1 - alpha) * K_map{low} alpha * K_map{high};逻辑说明车速越高动态响应越快增益整体呈下降趋势。插值让Kd随车速连续变化加减速工况下不会出现增益突变。注意这里的K_map是按车速网格离线算好的实车上只做线性插值开销可以忽略。6.2 验证方案是否合格三条曲线和一个扰动实验我的习惯是固定三轮验证圆形轨迹看稳态横向误差要求收敛到0.05m以内双移线变道看动态峰值误差0.3m以内算合格初始偏差实验给一个1m的横向阶跃偏差看收敛时间能不能小于2秒。做完这三项方案在覆盖范围内基本是放心的。部署到控制器之前所有代码里保持双精度浮点转C代码时留意float精度损失对e_y和delta的影响。量化误差在0.01rad量级时还能接受再大就要考虑改成double或者定点数。最后说一个踩出来的习惯调参时把每组Q、R和对应的峰值误差、峰值转角记录成表别在Matlab里随手覆盖数据。时隔一周再回来调前一轮的参数和效果查不到等于从头再来。这套方案之所以让我省心正是因为LQR只有两个旋钮每次改动的影响都能追溯到表格上。希望帮到你。本文还有配套的精品资源点击获取