简介最优控制理论在现代工程实践中扮演着关键角色其中线性二次型调节器LQR凭借其清晰的数学模型和物理可解释的参数设计被广泛应用于移动机器人、自动驾驶车辆及AGV的轨迹跟踪控制场景。LQR的核心思想是在系统状态空间模型的基础上通过最小化包含状态误差与控制能耗的代价函数求解出最优状态反馈增益从而在跟踪精度与执行器负担之间取得平衡。实际应用中车辆运动学模型以其参数少、结构简单、适合低速场景的优势成为LQR控制器设计的基础。通过将非线性运动学方程在参考轨迹附近线性化即可获得标准的状态空间表达式进而利用MATLAB的lqr函数快速求解反馈矩阵。借助Simulink平台工程师可以直观地搭建轨迹生成、车辆模型与控制器的联合仿真环境快速验证算法性能并调整权重参数。围绕LQR轨迹跟踪控制的完整链路从运动学建模、线性化推导、LQR参数整定到Simulink仿真实现进行系统梳理可为相关领域的算法研究与工程落地提供可复用的参考路径。 不是让读者死记公式而是真正理解LQR在轨迹跟踪里到底在干什么。在控制领域待久了你就会发现LQR这类最优控制方法之所以被大量用在移动机器人、自动驾驶、AGV这些场景里核心原因只有两个模型清晰、参数可直接对应物理含义。这篇东西我会从运动学建模开始一路推导到Simulink里能跑的仿真模型把每一步背后的逻辑讲明白方便你直接照着搭、照着调。1. 为什么是运动学LQR组合控制问题的分层解构1.1 这个项目到底在解决什么问题轨迹跟踪控制字面理解就是让车子沿着一条预先给定的路径走。这条路径可能是一条直线、一个圆形、一段双移线或者是某个规划器输出的离散轨迹点。难点在于车辆是一个非线性系统而且位置和姿态之间互相耦合——你想让车往左移一点方向盘打了之后它不会立马平移过去而是先转出一定的航向角再靠前进速度把位置拉过来。如果只用一个比例控制器直接对位置误差做反馈很容易出现两个问题一是转向过猛导致震荡二是稳态残留误差。PID虽然也能调但参数和车辆速度、转弯半径这些强耦合换一个工况就得重调一遍。LQR的思路完全不同。它在模型已知的前提下把控制设计变成一次最优化求解直接得到一组状态反馈增益让系统在线性化范围内达到性能和能耗的折中。换句话说LQR不是拍脑袋给增益而是让数学告诉你哪个增益最优。1.2 为什么选运动学模型而不是动力学模型控制算法设计的第一步是建模。很多人一上来就想建一个复杂的动力学模型把轮胎侧偏、惯性、横摆力矩全考虑进去。这个思路本身没错但如果你只是做轨迹跟踪算法的研究、验证、课程项目或者做的是低速移动机器人AGV、扫地机器人、室内送餐车那完整的动力学模型反而是负担。原因有三方面第一动力学模型参数多每一个轮胎刚度、转动惯量都需要实测或者辨识参数不准的情况下动力学模型反而不如运动学模型可靠。第二低速场景下加速度很小惯性力可以忽略车辆的运动行为基本由几何关系决定运动学模型已经能描述得很准。第三LQR本质上是在工作点附近做线性化运动学模型的线性化形式简单直观方便你直接看到A矩阵、B矩阵里每一项的物理来源。运动学模型不是凑合而是这个应用场景下的合理抽象。就像你算全球航线不会去考虑飞机机翼的具体形变一样不同层级的问题用不同精度的模型这就叫分层解构。1.3 LQR在这个框架里的价值运动学模型解决了车怎么动的问题接下来的问题是怎么让车按照期望的轨迹动。这里需要一个控制器来决策前轮转角。LQR能在这个环节发挥作用靠的是它三个核心特性一是状态反馈。它会同时利用位置误差和航向角误差来计算控制量而不是单一地看横向偏差这让系统具备更好的动态协调能力。二是最优性。LQR通过最小化一个包含状态误差和控制能耗的代价函数在跟踪精度和控制能耗之间做平衡。你可以在代价函数里告诉控制器我更在意横向误差还是更在意转向角别太猛。三是时间不变性或慢时变性。在固定工作点下算出来的反馈增益K可以直接复用就算工作点因为参考轨迹变化而改变只要轨迹参数的更新频率远低于控制频率在线实时计算A、B矩阵并重新解算K消耗的资源也依然可控。这三个特性组合起来就形成了一套既有理论根基、又能工程落地的控制方案。理解了这一层后面建模和仿真就是在给这个方案填细节。2. 车辆运动学模型从几何关系到状态空间表达2.1 自行车模型的本质假设在开始推公式之前先要讲清楚我们用的模型长什么样。这里用的是经典的自行车模型它把四轮车等效成两轮车前轮负责转向、后轮负责驱动车体看作一根刚性连杆连接前后轴。这个模型有三个关键假设忽略车辆的侧向滑移即每个轮的瞬时速度方向与轮子朝向一致。这在低速、干燥地面上近似成立。前后轴距集中在参数L上包含在模型里。车辆只在平面内运动不考虑俯仰和侧倾。在这三个假设下车辆的状态只需要三个量[x, y, θ]分别代表后轴中心的X坐标、Y坐标和航向角。控制输入通常是两个[v, δ]v是后轴中心的前进速度δ是前轮转角。模型的状态方程是![公式1]其中L是轴距。这组方程的物理含义非常直观x方向速度等于合速度的X分量也就是v乘以cos(θ)。y方向速度等于合速度的Y分量也就是v乘以sin(θ)。航向角变化率由几何关系决定前轮转角δ把前轮速度方向掰偏车辆绕着后轴做一个瞬时圆弧运动角速度等于v·tan(δ)/L。注意第三个式子里的tan(δ)是非线性的这决定了整个系统的非线性本质。但好消息是当我们把它展开到工作点附近时这个非线性项可以线性化后面LQR才有用武之地。2.2 状态方程与参考偏差的线性化LQR能直接处理的是线性系统也就是形如 \dot{x} A x B u 的方程。上面的运动学方程明显是非线性的所以我们必须在参考轨迹的每个工作点附近做线性化处理。假设参考轨迹由[xr, yr, θr]表示参考控制输入为[vr, δr]。把当前状态与参考状态的误差定义为![公式2]然后对非线性方程在参考点处做一阶泰勒展开忽略高阶项就得到误差状态的线性微分方程![公式3]具体展开后A和B矩阵分别是A [0, 0, -vr·sin(θr); 0, 0, vr·cos(θr); 0, 0, 0]B [cos(θr), 0; sin(θr), 0; tan(δr)/L, vr/(L·cos²(δr))]这个线性化结果值得认真看几遍因为它决定了控制器的设计方式。矩阵A里第三行全是0说明航向角误差本身没有自恢复特性——如果车头偏了它不会自己转回来必须有外部控制输入去纠正。矩阵B的第二列是转向角对航向角误差的影响通道数值是vr/(L·cos²(δr))可以看出速度越高、轴距越小同一个转向角产生的航向角变化率就越大。在实际仿真中如果你采用定速巡航的模式也就是恒定前进速度v那么控制输入可以只取前轮转角δ。这时候误差模型可以进一步简化为A [0, 0, -v·sin(θr); 0, 0, v·cos(θr); 0, 0, 0]B [0; 0; v/L] 注意此时B的第一个和第二个元素为0因为位置误差不直接被转角直接控制只能通过航向角影响这个简化模型特别适合用来做LQR验证因为系统结构简单三个状态中只有航向角受控制输入直接驱动位置误差是航向角误差的积分。控制器的任务就是通过合理设计反馈增益让这三个状态在相互耦合中快速收敛。2.3 模型离散化与仿真步长的关系在Simulink里做仿真时连续时间状态方程可以直接用Integrator模块搭建。但如果你需要把控制器部署到单片机上就必须把连续模型离散化成差分方程的形式。离散化的常用方法是一阶欧拉法或者零阶保持器法。对于运动学模型来说一阶欧拉法配合10ms的步长已经足够精确。离散形式为x(k1) x(k) v·cos(θ(k))·Ts y(k1) y(k) v·sin(θ(k))·Ts θ(k1) θ(k) v·tan(δ(k))/L·Ts这里的Ts就是控制周期。这个离散形式特别适合我们在后续Simulink模块中直接实现。另外有一个经验控制周期Ts应该比车辆动态时间常数小5到10倍。对于典型的室内移动机器人轴距0.3~0.5m、速度0.5~1m/sTs取0.01s到0.02s是合理区间。如果Ts太大离散化误差会明显影响LQR增益的适用性控制效果会变差。3. LQR核心推导代价函数、Riccati方程与反馈增益3.1 代价函数到底在优化什么LQR的全称是Linear Quadratic Regulator线性二次型调节器。它针对线性系统![公式4]设计控制律 u -K x使得如下的代价函数最小![公式5]这个代价函数有两部分。第一部分是xᵀQx衡量状态误差的累积大小。第二部分是uᵀRu衡量控制能量的累积消耗。矩阵Q和R不是自动确定的值而需要设计者根据实际需求来设定它们直接决定了控制器的性格。如果你把Q中的某个状态权重调大控制器就会更拼命地把这个问题状态压到零代价是控制量可能很大。如果你把R调大控制器就会变得吝啬每次转向都尽量轻踩轻放代价是收敛变慢。在轨迹跟踪场景中Q矩阵通常取为对角阵对角线上的三个值分别对应x误差、y误差、θ误差的权重。R是控制量前轮转角的权重是一个标量。3.2 Q和R矩阵的选取逻辑与方法Q和R的选取没有一个万能公式但有清晰的调参逻辑。我的建议是先在仿真里从以下初值开始Q diag([10, 10, 1])R 0.5这里的含义是横向位置误差和纵向位置误差的惩罚是航向角误差的10倍控制能耗惩罚相对宽松。也就是说我优先保证车辆贴线行走允许转向更主动一些。然后观察仿真曲线按照以下的规则调整现象原因调整方向收敛太慢误差几十秒才消除位置误差权重太低增大q_x、q_y如到50、100高频震荡转向角来回抖动控制权重太低或航向权重太高增大R或减小q_θ稳态有残留误差偏大位置惩罚不够或前馈缺失增大位置权重或增加前馈项转弯处误差明显变大速度过快或轨迹曲率大模型线性化误差变大适当减速或增大q_y一种更稳妥的做法是采用工程上常用的设计-仿真-调整循环而不是一上来就追求最优。每轮只改一个参数观察它对输出的影响积累出适合你手头模型的感觉。我在做四轮差速无人车时最后定下来的参数是Q diag([50, 50, 3])、R 2折线入弯时最大横向误差能做到5cm以内测试速度1.2m/s。3.3 MATLAB中lqr函数的使用与离线/在线增益MATLAB里求解LQR非常简单核心调用是K lqr(A, B, Q, R)这个函数内部求解的是连续时间Riccati方程AᵀP PA - PBR⁻¹BᵀP Q 0求得正定矩阵P之后反馈增益就是K R⁻¹BᵀP拿到K之后控制律就是u -K·e u_ref这里面u_ref是参考控制量在轨迹跟踪中就是参考前轮转角δr这保证了在误差为零的时候控制器输出的不是零而是维持车辆沿着参考轨迹行驶所需的那个基础控制量。关于增益的计算时机有两种工程处理方式离线计算如果参考轨迹固定比如就是圆形或直线可以在MATLAB脚本里先算好K然后作为Simulink模型里的常量使用。这种方式简单可靠适合教学和固定工况验证。在线计算如果参考轨迹曲率变化较大A矩阵和B矩阵随时间变化可以在Simulink的MATLAB Function里实时根据当前参考点计算A、B再调用lqr函数解算K。这种方式更接近自动驾驶场景中的实际做法但对嵌入式平台的计算性能有一定要求。实测下来对于长度几十米、曲率连续的参考轨迹在线逐点求解lqr的开销并不大控制周期10ms完全跑得动。这里有一个直接能用的离线计算脚本示例% 车辆参数 v 1.0; % 巡航速度 m/s L 0.5; % 轴距 m % 参考工作点以直线轨迹为例θr0, δr0 xr 0; yr 0; thetar 0; deltar 0; % 线性化矩阵 A [0, 0, -v*sin(thetar); 0, 0, v*cos(thetar); 0, 0, 0]; B [0; 0; v/L]; % 只有前轮转角为控制输入 % 权重矩阵 Q diag([50, 50, 3]); R 2; % 求解LQR反馈增益 K lqr(A, B, Q, R); % 输出K disp(K);运行这段脚本后得到的K就是Simulink模型里控制器要用的核心参数。如果参考轨迹不是直线A和B矩阵需要按当前参考点的θr、δr计算这点在后面Simulink建模时会再展开。4. Simulink建模实操从模型搭建到代码生成4.1 整体框架轨迹生成、车辆模型、控制器三子系统在Simulink里搭建LQR轨迹跟踪仿真我的习惯是把模型拆成三个子系统参考轨迹生成、车辆运动学模型、LQR控制器。三者之间的信号流向是参考轨迹生成模块输出期望状态[xr, yr, θr]和参考控制量[δr]车辆运动学模型输出实际状态[x, y, θ]控制器接收这两组数据的误差计算前轮转角δ输出给车辆模型。这样拆的好处有两个一是每个子系统都可以单独验证轨迹生成对不对、车辆模型积分是否正确、控制器的输出逻辑是否合理分开排查问题效率高二是后续如果要换参考轨迹只需要改轨迹生成模块控制器和车辆模型不用动。在Simulink模型里用Subsystem模块把这三个部分分别封装起来信号线用Goto/From或者直接连线都可以我更喜欢直接连线因为图上能一眼看出信号流向。4.2 参考轨迹生成从直线到圆周到任意曲线参考轨迹的生成方式取决于你想验证什么内容。直线轨迹是入门首选。设定xr v·tyr 0θr 0δr 0。车辆从某个初始偏差位置出发看控制器能不能把位置和姿态都拉回到这条直线上。圆形轨迹更适合检验LQR在连续曲率条件下的跟踪能力。设定圆心为(xc, yc)、半径为R角速度为ω v / R则xr xc R·cos(ωt) yr yc R·sin(ωt) θr ωt π/2 逆时针运动时 δr atan(L / R)注意这里的δr不是零而是维持圆周运动所需的恒定前轮转角。在直线轨迹中参考转角为0在圆形轨迹中它不是0这正是我前面强调u -K·e u_ref中u_ref重要的原因。在Simulink中生成这些信号最简单的方式是用Clock模块取时间t再接一个MATLAB Function模块在里面计算参考值。比如圆形轨迹的核心代码function [xr, yr, thetar, deltar] ref_circle(t, v, R, L) xc 0; yc 0; omega v / R; xr xc R*cos(omega*t); yr yc R*sin(omega*t); thetar omega*t pi/2; if abs(R) 0.01 deltar atan(L / R); else deltar 0; end end如果后面想挑战更复杂的轨迹比如双移线换道工况可以分段生成或用多项式曲线。但基础验证阶段直线加圆形两个工况已经完全足够说明LQR的性能。4.3 车辆运动学模型用Integrator模块搭积分链在Simulink里搭建运动学模型的思路是把三个微分方程各用一个积分器实现。分别画出x_dot、y_dot、θ_dot的计算然后接入Integrator模块积分得到x、y、θ。具体模块连接如下输入端口v巡航速度常量或外部信号、delta前轮转角来自控制器计算x_dot v·cos(θ)用Trigonometry模块选cos计算y_dot v·sin(θ)用Trigonometry模块选sin计算θ_dot v·tan(delta)/L用Trigonometry模块选tan三个Integrator模块分别积分初始条件设置为车辆的初始位姿[x0; y0; θ0]有一个细节需要注意如果初始航向角θ0设置得和参考航向角θr0差得比较大比如超过60度线性化模型的近似精度会下降控制效果会变得很怪。这种情况需要要么把初始误差限制在小角度范围内要么先加一个粗对准逻辑。常规测试中初始航向角误差控制在30度以内比较稳妥。4.4 LQR控制器的实现MATLAB Function内部逻辑控制器是模型的核心。我倾向于用MATLAB Function模块实现LQR控制器输入是当前状态、参考状态和参考转角输出是实际前轮转角。控制器内部的核心代码逻辑如下function delta lqr_controller(x, y, theta, xr, yr, thetar, deltar, v, L, K) % 状态误差 ex x - xr; ey y - yr; etheta theta - thetar; % 将航向角误差归一化到[-pi, pi]避免角度跳变 etheta atan2(sin(etheta), cos(etheta)); % LQR反馈控制律 delta deltar - K(1)*ex - K(2)*ey - K(3)*etheta; end这个代码里有一个很关键的小细节——航向角误差的归一化。因为在Simulink仿真中θr会随时间增大比如圆周轨迹θ也在不断变化如果直接做减法可能会出现类似359度和0度的差值被算成359度而不是1度的问题。用atan2(sin(), cos())把角度差归一化到-pi到pi之间可以彻底避免这个坑。K值可以在MATLAB脚本里预先算好然后通过Model Workspace或直接以Parameter形式传给MATLAB Function。也可以把A、B矩阵的计算和lqr调用搬进控制器内部实时求解但那样每个控制周期都会做Riccati方程的数值求解仿真速度会明显变慢。我的建议是先用离线K值跑通仿真再考虑改成在线求解。4.5 仿真配置与控制参数输出Scope波形怎么看搭完模型后仿真配置也要注意。在Simulink的Model Configuration Parameters里建议做如下设置求解器类型选固定步长Fixed-step求解器选ode4经典四阶龙格库塔固定步长大小设为0.01对应100Hz的控制频率仿真时间根据轨迹长度设定比如圆形轨迹速度1m/s、周长约12.6m仿真时间设15s足够为了查看控制效果需要在模型中添加几个Scope横向误差ey随时间变化的曲线——最核心的指标车辆实际轨迹和参考轨迹的XY图用XY Graph模块可以直观看到贴合程度前轮转角δ的输出曲线确认没有震荡和饱和其中横向误差的观察要特别注意坐标系问题。我前面实现里ey y - yr是全局坐标系下的Y方向误差。在直线沿着X轴的场景下这个值就等于横向偏差。但如果参考轨迹是一条斜线或曲线这个误差就不是严格意义上的距轨迹的垂线距离。更严谨的做法是将状态转换到轨迹的Frenet坐标系或者用向量叉积计算横向偏移量。在入门验证时先用全局坐标系下的Y方向误差做指标基本场景已经够用。5. 仿真调试中高频踩坑点发散、振荡与参数敏感性5.1 模型初始化与初始航向角误差过大我见过最多的问题就是初始化阶段的发散。很多人把车辆初始位姿设成(0, 0.5, 0.1)参考轨迹起点在(0, 0, 0)看起来误差不大但一跑起来前半段却出现大幅偏移甚至发散。原因在于线性化模型是基于参考工作点展开的。当初始航向角误差超过30~40度时一阶线性近似的误差已经很大LQR控制器基于这个近似模型算出的反馈增益在真实非线性模型上表现就会失稳。具体表现就是位置误差还没收敛航向角先开始大幅摆动。解决办法有三种把初始姿态误差限制在小角度范围内比如让初始θ0和θr0的差不超过20度。在实际系统中做一个预对准步骤先用较弱的控制器或手动控制把车头大致转到参考方向再切换LQR。在控制器输出上加限幅比如前轮转角限制在±0.5rad以内避免控制器在异常状态下输出过大的转角。三种方法中限幅是最容易实现也最应该优先做的。无论怎么调参数前轮转角都不可能无限大物理机构决定了它有一个上限。在Simulink中加一个Saturation模块直接限制输出范围可以有效避免模型数值发散。5.2 增益过大导致的高频振荡与转向抖动另一个常见的现象是车辆轨迹出现锯齿状的抖动尤其是在接近参考轨迹时。位置误差被压得很小但前轮转角还在来回切换像是驾驶员在小范围内快速修正方向。这种情况的根因通常是K值偏大或控制频率不够。具体来说是Q矩阵中的位置权重设得过高或者R值设得太低。我调试时的一个经验法则是K矩阵中的位置增益K(1)、K(2)如果超过10就很容易引入振荡。如果转向角输出曲线出现了接近正弦的高频分量优先检查R是否太小。如果位置误差已经小于0.01m还在抖说明控制器过于敏感可以认为已经过拟合于噪声信号。遇到振荡不要急着堆滤波器先用参数扫描找规律。在MATLAB脚本里写一个循环让R从0.1变到10分别仿真并记录位置误差的均方根值画出来观察曲线的拐点。最优参数往往在误差开始下降变缓的那个位置附近。5.3 参考输入前馈的价值圆形轨迹跟踪中不可忽略的u_ref下面这个例子很能说明问题。用同一组K值分别跟踪直线轨迹和圆形轨迹如果控制器里没有加上参考前轮转角δr这一项圆形轨迹的跟踪会产生明显的稳态误差车辆始终会切向偏离一个固定角度。原因很简单维持圆形轨迹本身就需要一个持续的转向角δr atan(L/R)。如果控制律只是u -K·e那么当车辆超调回到参考轨迹上时误差为零控制器输出也为零前轮转回正前方。但正前方的车是不可能走出圆形轨迹的于是车辆又往外漂直到产生新的误差来补偿缺失的偏转量。这也是我为什么在控制律里反复强调u -K·e u_ref的原因。u_ref是模型对维持参考运动所需控制量的预测反馈部分只是修正偏差。前馈反馈的组合让控制器不仅会纠偏还能主动适应轨迹特性。在直线轨迹中δr 0所以这个前馈项不显现。一旦换成圆形或曲线轨迹这个加法项的效果立刻显现出来。5.4 控制量饱和与执行器限制仿真外的现实约束仿真里一切正常不代表实车就能跑。最容易被忽略的就是执行器饱和问题。前轮转角在数学上可以取任意值但实际舵机或转向电机都有物理限位。如果LQR算出的期望转角超过了执行器能实现的极限实际转角会被截断车辆的真实响应会和控制器假设的模型不一致严重时会导致控制性能下降甚至不稳定。处理办法是在Simulink里就加入Saturation模块模拟执行器限位同时在设计LQR时把R权重加大让最优解本身尽量落在执行器能力范围内。这个方法虽然不能完全避免饱和但至少能让控制器学会量力而行。我在做两轮差速机器人平台时把转角限位设为±0.35rad速度设为0.8m/sQ diag([30, 30, 1])R 1.5效果比较理想。横向稳态误差能控制在0.02m以内转弯处最大误差在0.05m左右。如果你的平台速度更快建议同时降低位置权重或者提高R值因为高速下同等转角会产生更大的横向加速度控制器需要更温和的动作来避免过冲。调试LQR轨迹跟踪说到底就是和模型、参数、约束逐步较劲的过程。我先用离线算好的K值在固定轨迹上跑通整个链路再逐项加入限制条件和扰动每一次改动都只碰一个变量这样才能看清每个环节的真实影响。上面这些经验组合在一起应该能让你少走不少弯路。本文还有配套的精品资源点击获取