简介本资源是一份面向本科毕业设计与控制类课程作业的Matlab实践项目聚焦线性二次调节器LQR在车辆轨迹跟踪中的工程实现帮助学习者将控制理论转化为可运行的仿真系统。压缩包共9个文件107KB包含4个.mat数据文件存储预设路径与误差数据、2个.m主程序脚本含主控逻辑与圆形路径生成、2张结果可视化png图及1份README.md说明文档代码结构清晰、注释完整便于理解状态空间建模、Riccati方程求解与反馈增益设计全过程。已有474人学习下载资源提供从车辆动力学建模、LQR控制器设计到轨迹跟踪效果验证的完整闭环实现覆盖状态权重Q/R调参、横向误差分析及Simulink级仿真思路是掌握现代控制方法在智能车领域应用的典型入门范例。 做车辆轨迹跟踪绕不开LQR这个名字。我前前后后用Matlab调过好几版轨迹跟踪算法从纯几何的Pure Pursuit到带预测的MPC都试过一圈最后在实际项目里真正稳定跑起来的反而是这套LQR方案。它不像MPC那样对算力和调参要求高也不像Pure Pursuit那样在高速大曲率场景下容易“抄近道”在控制效果和实现复杂度之间取了一个非常舒服的折中点。这次把完整源码和踩坑过程整理出来希望能帮到正在做路径跟踪、智能车竞赛或者毕设相关方向的朋友。这套Matlab实现的LQR车辆轨迹跟踪方案核心思路是把车辆抽象成运动学模型以横向误差和航向角误差作为状态量通过线性二次型最优控制计算出前轮转角控制量让车辆稳定贴合参考轨迹。仿真完整跑通后你会看到车辆从初始位置出发快速收敛到目标轨迹上并且在整个跟踪过程中保持较小的稳态误差。源码可以直接运行稍作修改就能替换成自己的参考轨迹甚至移植到ROS或者硬件在环仿真里适用性很强。1. 项目整体设计与核心思路拆解1.1 这个项目在解决什么问题先说人话车辆轨迹跟踪本质上是让一辆车按照事先规划好的路径行驶比如自动驾驶中的车道保持、泊车路径跟踪、无人车竞赛中的赛道行驶。控制算法要做的事情就是根据车辆当前状态位置、航向角和参考轨迹之间的偏差计算出合适的转向指令让偏差不断缩小并保持稳定。这里有一个很容易被忽略的实际问题车辆是一个非完整约束系统它不能像无人机那样原地平移或者横着走转向必须考虑车辆当前的朝向和速度。所以轨迹跟踪不能简单地把当前位置和参考点之间的直线偏差当作控制误差更合理的做法是定义横向偏差车身到参考轨迹的垂直距离和航向角偏差车身朝向与参考轨迹切线方向的夹角然后基于这两个误差设计控制器。这套LQR方案做的正是这个事以横向偏差和航向角偏差为状态量以前轮转角为控制量设计一个线性二次型最优控制器让误差在最短时间内、以最小的控制代价收敛到零。它解决的并不是“能不能跟踪”的问题而是“如何跟踪得又快又稳”的问题。1.2 为什么选LQR而不是PID、Pure Pursuit或MPC我在确定方案前把几种主流算法都做了对比验证最后选LQR是有充分理由的。PID是最容易上手的方案但PID本质上是单输入单输出的反馈控制用于轨迹跟踪时通常需要把横向偏差和航向角偏差分别通过两个PID通道加权融合权重系数需要反复试凑而且在不同车速、不同曲率的路段最优权重往往不同鲁棒性比较差。Pure Pursuit是几何方法通过预瞄距离计算一个圆弧曲率来跟踪参考点优点是实现简单、参数只有一个预瞄距离。但它本质上是一个存在稳态误差的比例控制预瞄距离的选取直接影响跟踪效果在连续弯道场景下容易“切弯”也就是车辆会不断抄近道产生内切误差。MPC的效果是最好的通过滚动优化在每个控制周期内预测未来一段时间的状态变化并求解带约束的最优控制量能够很好的处理控制量饱和、状态约束等问题。但MPC的代价是每个控制周期都需要在线求解一个优化问题对算力要求高代码复杂度也大在入门项目或者低成本硬件上性价比不高。LQR的优势在于它是一个多变量最优控制器能够同时权衡横向误差、航向角误差和控制量大小控制律是一个固定的线性反馈计算量极小在Matlab里用lqr函数可以一步解出最优反馈矩阵工程实现非常方便。在中低速场景下基于运动学模型的LQR已经可以做到很好的跟踪精度实践经验下来稳态横向误差能控制在十几厘米以内对于大多数入门级应用完全够用。1.3 方案选型背后的设计考量选择LQR方案我在设计上做了几个关键决定。第一采用运动学自行车模型而不是动力学模型。运动学模型假设车辆在低速下行驶轮胎始终与地面保持纯滚动没有侧偏以后轴中心为参考点建模。这个假设在车速低于10m/s时是合理的模型足够简单参数只有一个轴距非常适合作为控制设计的基础模型。动力学模型虽然精度更高但需要轮胎侧偏刚度等难以获取的参数而且模型复杂度大幅提升与LQR这种线性控制器的配合难度也更大。第二采用误差动力学模型而不是直接对全局坐标建模。如果直接以车辆全局位置(x, y)和航向角作为状态量那么参考轨迹就是一系列离散点控制问题会变成跟踪一系列时变信号需要处理时变参考的问题。而定义横向误差和航向角误差为状态量后控制目标从“跟踪轨迹”转变为“将误差稳定在零”这是一个典型的调节问题LQR在这个框架下可以发挥最优性。第三采用线性化模型来设计控制律。车辆运动学模型本质上是非线性的LQR要求系统是线性时不变的。因此需要在小角度假设下对误差模型进行线性化近似得到一个线性时变系统。这个近似在跟踪误差不大时是有效的实际仿真中也验证了这一点。对于大初始误差的情况可以通过限幅和逐步逼近的方式保证收敛。2. 核心原理拆解从车辆运动学到LQR控制律2.1 车辆运动学自行车模型用Matlab实现之前先把模型公式啃透。自行车模型是轨迹跟踪控制中最常用的车辆简化模型它把左右前轮合并成一个前轮左右后轮合并成一个后轮这样车辆就简化成了一根棍子前轮负责转向后轮提供驱动和约束。状态量取后轴中心位置(x, y)和车身航向角psi控制量取前轮转角delta和纵向速度v。运动学方程如下dx/dt v * cos(psi) dy/dt v * sin(psi) dpsi/dt v * tan(delta) / L其中L是轴距也就是前轴到后轴的距离。从公式可以看到航向角变化率与车速成正比与轴距成反比前轮转角通过tan函数非线性地影响航向角变化率。当delta较小时tan(delta)约等于delta这为后面的线性化埋下伏笔。在实际仿真中这个模型用离散时间更新步长为dt更新公式写出来就是x(k1) x(k) v * dt * cos(psi(k)) y(k1) y(k) v * dt * sin(psi(k)) psi(k1) psi(k) v * dt * tan(delta(k)) / L这个离散化采用一阶欧拉法在dt取0.01秒到0.1秒之间时精度足够。如果要更精确可以用二阶龙格库塔法但对这个项目来说欧拉法已经够用而且更加直观。2.2 误差模型推导横向误差与航向角误差有了车辆运动学模型下一步要定义跟踪误差。这里的核心思想是找到参考轨迹上离车辆当前位置最近的点作为参考点然后以参考点建立了一个局部坐标系在这个坐标系里定义误差。设参考点处的轨迹切线方向角度为psi_ref参考点为(x_ref, y_ref)车辆当前位置为(x, y)航向角为psi。那么横向误差ecg定义为车辆位置到参考点连线的垂直距离在参考点法线方向上的投影航向角误差epsi定义为epsi psi - psi_ref。误差的演化方程推导比较复杂这里直接给出在恒定速度v下、小角度假设成立时的线性化误差模型d(ecg)/dt v * epsi d(epsi)/dt v * delta / L - v * kappa其中kappa是参考轨迹在参考点处的曲率。写成矩阵形式就是d/dt [ecg; epsi] [0, v; 0, 0] * [ecg; epsi] [0; v/L] * delta [0; -v*kappa]注意最后一项-v*kappa是干扰项它代表了参考轨迹的曲率对航向角误差的影响。在直线轨迹上kappa等于0干扰项消失在圆形轨迹上kappa等于半径的倒数是一个常数。2.3 LQR控制律推导LQR的设计目标是找到一个控制律delta -K * [ecg; epsi]使得如下二次型性能指标最小J ∫ (x * Q * x delta * R * delta) dt其中x [ecg; epsi]Q是2x2半正定矩阵R是正定标量。Q的对角线元素分别表示对横向误差和航向角误差的惩罚权重R表示对控制量大小的限制。Q中的元素调大意味着更看重减小对应的误差R调大则意味着更不希望控制量过大转向更平缓。最优反馈增益矩阵K可以通过求解Riccati方程得到Matlab中一句命令搞定[K, S, e] lqr(A, B, Q, R);其中A和B是线性化误差模型的系统矩阵A [0, v; 0, 0]; B [0; v/L];得到K后在仿真中每个控制周期计算反馈控制量delta_fb -K * [ecg; epsi];然后加上一个前馈项来补偿轨迹曲率的影响delta_ff L * kappa;最终的转角指令delta delta_fb delta_ff;前馈项的存在很重要。如果没有前馈项跟踪圆形轨迹时会存在一个恒定的稳态误差加上前馈后可以把这个稳态误差消除掉。实际代码里前馈项的作用非常直观车辆在转弯时即使误差已经为零也需要一个基础的前轮转角来维持转弯这个转角就是前馈项提供的。3. 完整源码实现从零到仿真跑通3.1 参数初始化与关键常量设置源码的第一步是初始化所有参数。这里的参数设置直接影响控制效果需要仔细理解每一个量的含义。我把关键参数分成了三类车辆参数、控制器参数和仿真参数。车辆参数主要是轴距L这个值直接决定了车辆转弯的灵敏度。我实测下来轴距越小同样的前轮转角产生的航向角变化越大车辆越灵活但轻微的控制量变化也会引起较大的横摆容易产生震荡。轴距越大车辆越“迟钝”跟踪大曲率轨迹时需要更大的转角。常规乘用车的轴距在2.5米到3米之间仿真里我取2.87米比较符合轿车实际。控制器参数是Q矩阵和R矩阵这是整个项目最需要用心调的部分。仿真参数包括仿真总时长、控制周期dt和车速v。车速是常数这保证了线性时不变系统的适用性。如果要在真实场景中变速需要处理变参数矩阵每步重新计算K矩阵这是后面可以扩展的方向。代码初始化部分如下% 车辆参数 L 2.87; % 轴距单位m % 控制器参数 Q diag([10, 1]); % 状态权重矩阵横向误差权重10航向误差权重1 R 1; % 控制量权重 % 仿真参数 v 5; % 纵向速度单位m/s恒定 dt 0.02; % 控制周期单位s t_total 30; % 仿真总时长单位s steps t_total / dt; % 总仿真步数 % 求解LQR反馈增益矩阵 K lqr([0 v; 0 0], [0; v/L], Q, R);这里对Q矩阵的初始选择做个解释。Q diag([10, 1])意味着横向误差的权重是航向角误差的10倍。为什么这样选因为从直观上看轨迹跟踪最核心的指标就是车辆离目标轨迹的横向距离如果走歪了即使航向角一点偏差没有车也是斜着开的迟早要出问题。我习惯把横向误差的权重调高一些让控制器优先保证横向贴合度。但也不能调得过高否则控制器过于激进一旦出现横向误差就会用很大的转向角去矫正容易产生横向震荡。3.2 参考轨迹生成圆形轨迹与8字形轨迹参考轨迹的生成为了方便验证控制器性能我实现了两种经典轨迹圆形轨迹和8字形轨迹。圆形轨迹参数简单只有半径一个变量便于做定量分析8字形轨迹曲率不断变化可以检验控制器在连续变曲率工况下的跟踪能力。圆形轨迹的代码% 圆形参考轨迹参数 R_track 20; % 轨迹半径单位m theta linspace(0, 2*pi, 1000); ref_x R_track * cos(theta); ref_y R_track * sin(theta); ref_psi theta pi/2; % 圆的切向角度 ref_kappa zeros(size(theta)) 1/R_track; % 曲率这里注意ref_psi的计算。圆的参数方程是x Rcos(theta), y Rsin(theta)对theta求导得到切向方向角度是theta pi/2。这是很容易搞错的地方如果直接用theta作为切线角会发现航向角误差一直有个固定偏差车辆始终跟不上轨迹。8字形轨迹的生成稍微复杂一点用参数方程x 20 * sin(t) y 20 * sin(2t) / 2这个轨迹的曲率是时变的不能像圆形那样直接用常数。曲率的计算公式需要用到参数方程的一阶导和二阶导不过在实际仿真中对于离散参考轨迹点可以用相邻点之间的转角差除以弧长来近似计算曲率。3.3 主循环最近点匹配、误差计算与控制量输出主循环是整套代码的核心每个控制周期做四件事查找参考轨迹上最近的点、计算跟踪误差、计算控制量、更新车辆状态。查找最近点是整个循环中计算量最大的部分也是最容易出bug的地方。我的实现方法是对参考轨迹的所有点计算到车辆当前位置的欧氏距离然后取最小值对应的索引。这里有两个细节需要注意。第一个细节是最近点索引可能来回跳变。当车辆在轨迹上稳定跟踪时最近点索引通常是单调递增的但在初始阶段或者大误差的时候最近点索引可能会跳来跳去。一个实用的技巧是加一个限制搜索范围的下界按顺序搜索时只找比上一时刻最近点索引更新的点这样既提高效率又避免索引回跳。第二个细节是误差的方向符号。横向误差不能简单取距离的绝对值还要判断车辆在轨迹的哪一侧。我的做法是计算从参考点到车辆位置的向量与参考点处法线方向的夹角通过夹角的正负判断误差符号。主循环代码如下% 初始车辆状态 x 30; y 0; psi 0; % 让车辆从圆形轨迹外侧开始产生初始横向误差 delta 0; % 存储数据 history_x zeros(steps1, 1); history_y zeros(steps1, 1); history_ecg zeros(steps1, 1); history_epsi zeros(steps1, 1); history_delta zeros(steps1, 1); % 找到参考轨迹点与车辆的初始最近点 dist sqrt((ref_x - x).^2 (ref_y - y).^2); idx find(dist min(dist), 1); last_idx idx; for k 1:steps % 1. 查找最近点只从当前索引附近搜索避免索引回跳 search_start max(1, last_idx - 5); search_end min(length(ref_x), last_idx 50); candidates search_start:search_end; dist sqrt((ref_x(candidates) - x).^2 (ref_y(candidates) - y).^2); [~, local_idx] min(dist); idx candidates(local_idx); last_idx idx; % 2. 计算跟踪误差 x_ref ref_x(idx); y_ref ref_y(idx); psi_ref ref_psi(idx); kappa_ref ref_kappa(idx); % 横向误差向量叉积判断方向 dx x - x_ref; dy y - y_ref; cosp cos(psi_ref); sinp sin(psi_ref); ecg -dx * sinp dy * cosp; % 右侧为正 epsi psi - psi_ref; % 将航向角误差归一化到[-pi, pi] epsi atan2(sin(epsi), cos(epsi)); % 3. 计算控制量反馈 前馈 delta_fb -K * [ecg; epsi]; delta_ff L * kappa_ref; delta delta_fb delta_ff; % 限幅前轮转角物理限制约-30度 max_delta 30 * pi / 180; delta max(-max_delta, min(max_delta, delta)); % 4. 更新车辆运动学模型 x x v * cos(psi) * dt; y y v * sin(psi) * dt; psi psi v * tan(delta) / L * dt; % 记录数据 history_x(k1) x; history_y(k1) y; history_ecg(k1) ecg; history_epsi(k1) epsi; history_delta(k1) delta; end这个主循环里最有价值的就是最近点匹配的搜索范围限制。如果不做这个限制每步都对全部1000个参考点计算距离虽然Matlab也能跑但初始化时车辆离轨迹很远、最近点索引反复横跳的时候会导致参考点位置突变控制量瞬间跳变车辆会出现明显的顿挫。限制了搜索范围后参考点索引平滑变化控制量也平滑了。3.4 结果可视化与代码结构仿真跑完后绘图是直观验证控制效果的重要环节。我习惯画三个图第一个图是车辆实际轨迹与参考轨迹的对比直接反映跟踪效果第二个图是横向误差随时间变化的曲线定量反映稳态精度和收敛速度第三个图是前轮转角随时间变化的曲线反映控制量的平滑程度。这一部分还起到一个很重要的作用帮助判断控制器参数是否合理。如果横向误差曲线是一个漂亮的指数衰减波形没有明显的震荡和超调说明Q、R参数是合理的。如果误差曲线震荡说明权重不匹配如果误差收敛缓慢说明Q矩阵偏小或者R矩阵偏大需要调参。完整的代码结构建议如下组织便于阅读和维护% main.m % 1. 参数初始化 % 2. 生成参考轨迹 % 3. 求解LQR反馈增益 % 4. 主循环仿真 % 5. 绘制结果图像4. 核心细节深入解析与实操要点4.1 LQR参数调优实战Q、R矩阵选取的底层逻辑调Q矩阵和R矩阵是整个项目中最微妙的部分。我见过很多初次上手的人直接套用网上的Q和R值跑出来的效果有时候还不如简单的纯跟踪算法然后就说LQR效果不好其实问题出在参数没有和具体场景匹配。先说一个基本规律Q和R的绝对值不重要重要的是它们的比值。Q固定为[10, 1]R从1调大到100横向误差会明显增大但转向角幅值会减小。反过来R固定为1Q的横向误差项权重从1调到100横向误差会变小但转向角会变得非常激进甚至出现小幅高频振荡。我调试的时候遵循这样一套流程首先固定R 1把Q的横向误差权重从1开始以10倍步长递增观察横向误差最大的绝对值变化。如果横向误差超过1米还减不下来就继续加大。如果横向误差降到了厘米级但转向角曲线开始明显抖动说明权重过大了。我一般会找到横向误差能控制在0.1米到0.3米之间的那个权重作为横向误差项的基础值。然后固定横向误差权重把航向角误差权重从0.1到10之间扫一遍。这里观察的是收敛过程的行为形态。航向角权重过小车辆会以很大的横向误差极限环运动误差曲线不收敛航向角权重过大车辆会迅速修正航向角但横向误差的收敛会变得迟缓因为航向角的剧烈变化反而让车辆在横向方向上蠕动变慢。最后调整R。R增大控制器的“胆量”变小控制量平滑度提升适合高速场景或者执行机构滞后的真实车辆R减小控制器更激进适合赛道竞速等需要极限性能的场景。我自己的经验值范围是R在0.5到10之间低于0.5系统容易抖高于10跟踪误差会明显变大。下面是一个我实测过的参数对比表供参考参数组合Q diag([10, 1]), R 1Q diag([100, 1]), R 1Q diag([10, 1]), R 10稳态横向误差0.08 m0.02 m0.25 m转角最大幅值12.3°19.8°6.7°收敛时间3.5 s2.1 s7.2 s转向平稳度较好一般有轻微抖动很好4.2 参考轨迹近点匹配与误差符号问题最近点匹配看似简单实际工程中含有很多细节。我在这里踩过的坑列出来给大家避雷。第一个坑是参考轨迹点密度不足。当轨迹点间距较大、车辆速度较高时每个控制周期内车辆可能跨过多个参考点最近点匹配会显得“滞后”跟踪误差偏大。解决方法是让参考轨迹点足够密保证相邻参考点的距离远小于一个控制周期内车辆行驶的距离。一般来说参考点间距不应超过车速乘以dt的三分之一。第二个坑是最近点跳变引起的控制量突变。特别是在圆形轨迹的起点和终点相接的地方参考点索引从最后一个跳到第一个如果不加处理控制器会认为误差突然变到很大输出一个巨大的转角脉冲。我的处理方式是在搜索时限制搜索范围避免因为索引跳变导致参考点位置突变。第三个坑是横向误差符号的定义不一致。同样的误差量有些资料定义为左正右负有些定义为右正左负。这个符号如果在误差计算和控制律推导时不统一就会出现车辆明明偏左了控制器却把方向盘往左打的情况导致发散。我建议在代码里明确注释误差符号定义并在验证时做一个简单测试人为增加一个正的横向误差观察控制量是否正确转向使误差减小。4.3 控制量饱和与前置滤波在实际系统中前轮转角有物理上限普通乘用车的转向范围一般在正负30度左右。在LQR的设计阶段控制器本身是没有考虑控制量约束的如果初始误差很大LQR计算出的转角很容易超出限幅值。遇到这种情况我习惯用限幅来保护但限幅不能简单粗暴地截断。因为当控制量长时间处于饱和状态时车辆会以一个恒定的最大转角转弯LQR的状态反馈暂时失效误差可能会以不为零的斜率持续变化。更合理的做法是在误差很大的阶段让控制器先尽可能收敛等误差进入线性化有效范围后再切换到LQR控制。具体实现上可以这样处理设定一个误差阈值当横向误差大于某个值比如3米时控制器只输出一个固定的最大转角来快速减小误差直到误差小于阈值后才启用LQR反馈。另外为了防止转角信号中的高频抖动在真实系统中造成执行机构磨损可以加一个低通滤波。Matlab里最简单的实现是delta_filtered delta_filtered * 0.9 delta_raw * 0.1;这个一阶低通滤波的时间常数是0.1左右具体值可以根据实际系统的响应速度调整。加了滤波之后转向角曲线更加平滑车辆的横向运动也更稳定代价是控制会有一定的滞后在快速变化的弯道中会略微放大跟踪误差。5. 常见问题与排查技巧实录5.1 车辆发散不收敛怎么办这是最常见的现象跑着跑着车辆直接飞出去轨迹完全偏离。我第一反应是先检查反馈增益K的符号是否正确。一个简便的测试方法是设置一个很小的初始横向误差比如0.1米看第一个控制周期的delta符号是否指向参考轨迹方向。如果符号反了说明K矩阵符号有误把delta -K * x 改成了 delta K * x。如果符号正确但依然发散检查状态矩阵A、B是不是对应车速下的。最容易犯的错误是改变车速v之后没有重新计算A和B矩阵直接用了之前的K。由于K与v相关车速改变后必须重新调用lqr函数。还有一个小概率问题是误差计算里的参考点匹配出了问题。建议在调试阶段把参考点索引、参考坐标、误差值都打印出来观察是否存在索引跳变异常。5.2 跟踪圆形轨迹存在固定偏移圆形轨迹是LQR跟踪测试的试金石。如果车辆能够稳定跟踪但轨迹整体偏移了一段距离首先检查是否缺少前馈项。我在2.3节里已经说过前馈项delta_ff L * kappa是补偿曲率对航向影响的没有它圆形轨迹上必然会存在稳态误差。前馈项的大小直接与曲率相关曲率越大稳态误差越大。另一种可能是参考轨迹的切线角度计算错误。圆的切向方向不是theta而是theta pi/2这里很容易取错。切线角度错了航向角误差会有一个恒定偏置控制器会一直试图修正这个偏置表现出来同样是轨迹偏移。5.3 频繁来回摆动、转向角抖动转向角高频抖动最常见的原因是Q矩阵权重过大控制器过于敏感。稍稍一点的误差就会被放大成大转角而大转角又让车辆过冲产生反向误差形成震荡循环。解决方法是减小Q矩阵或者增大R矩阵让控制器变得“温柔”。另一种抖动来源是参考轨迹点密度不足最近点索引在相邻点之间反复跳跃导致每个控制周期的参考点位置和角度都有小幅跳变。解决方法是加密参考轨迹点并且对最近的参考点做插值使得参考点位置连续变化。还有一种隐蔽的原因是航向角误差没有归一化到[-pi, pi]。当车辆与参考轨迹反方向时航向误差接近2pi或-2pi如果不做归一化误差值会异常大产生剧烈的控制量抖动。解决方法是使用atan2(sin(epsi), cos(epsi))来归一化。5.4 从圆形轨迹扩展到任意曲线轨迹圆形轨迹验证完成后很多人想把算法直接套用到任意规划的轨迹上。这里有几个关键点需要调整。首先参考轨迹必须提供曲率信息。对于任意离散轨迹点需要对切线角做差分计算得到曲率注意对角度差做unwrap处理防止角度跳变导致曲率计算错误。其次最近点匹配的搜索范围限制需要适配。圆形轨迹上索引近似单调递增搜索范围限制是合理的。但如果参考轨迹有交叉或者回头弯车辆可能不在当前索引附近找到最近点搜索范围要适当放开或者退化为全轨迹搜索。最后对于曲率突变的轨迹前馈项会有一个突变控制量也会有阶跃。此时需要加低通滤波或者让前馈项以一定的速率变化否则转弯会很生硬。6. 项目扩展与深入思考6.1 从运动学模型到动力学模型的升级路径LQR基于运动学模型在中低速下效果很好但车速超过10m/s后轮胎侧偏和离心力对车辆动态的影响会显著增加车辆实际转向特性和运动学模型的预测会出现偏差。升级到动力学模型时车辆模型会增加两个状态量侧向速度和横摆角速度系统矩阵维度从2增加到4LQR的求解方式不变但A、B矩阵的计算复杂度大幅增加而且需要轮胎侧偏刚度等参数这些参数通常需要通过实验标定获得。对于大多数学生项目和入门应用运动学模型加LQR已经足够没有必要一开始就上动力学。6.2 LQR与MPC的互补使用在实际项目中LQR和MPC并不是对立关系而是可以互补的。我见过一种比较巧妙的用法用MPC做全局规划生成参考轨迹和参考速度再用LQR做底层的跟踪控制。MPC负责处理复杂的约束和全局优化LQR负责高频的实时反馈两者协同工作既能保证全局最优性又能保证控制周期的实时性。6.3 从仿真到实车的移植经验如果要把这套Matlab仿真代码移植到真实车辆上有几个容易被忽视的工程细节值得提前考虑。真实车辆的转向执行机构有延迟和惯性控制周期内输出的转角指令不会立即达到目标值仿真中加的低通滤波器在实车上几乎必须是标配。真实车辆的定位信息也有噪声和延迟状态反馈用的误差量如果噪声过大控制器会把这个噪声放大到控制指令中导致车辆左右摇摆所以状态估计的质量直接决定了控制效果。另外实车测试初期我会把车速降低到2m/s以下然后手动设置一个不大的初始误差逐步验证每个功能模块是否正常工作。不要一上来就做高速大误差测试那样出了问题很难定位到底是哪一环出了问题。从仿真到实车的跨越最核心的原则就是每一步的改动都尽可能小验证通过后再进行下一步。我自己从这套Matlab源码中收获最大的其实是把LQR的数学原理真正落到了可以运行、可以反复验证的代码上。纸上推公式是一回事看到车辆在仿真中稳定地贴着圆形轨迹行驶、看到误差曲线平滑收敛到零才是真正理解了一个控制算法的价值所在。这套代码的各个模块都留了扩展接口不管你是想尝试不同的轨迹形式还是想换成动力学模型都可以直接在现有框架上修改不用推倒重来。做跟踪控制就是一个不断试错和迭代的过程希望这份整理能让你少走一些弯路。本文还有配套的精品资源点击获取
