基于运动学模型的LQR轨迹跟踪控制：从模型推导到Simulink仿真实践

简介在自动控制领域状态反馈控制是设计高性能闭环系统的核心手段之一。线性二次型调节器LQR作为一种最优控制策略通过最小化包含状态误差与控制能量的代价函数可系统性地求解最优反馈增益矩阵。在移动机器人、智能车辆等轨迹跟踪场景中常采用自行车运动学模型描述车辆位姿变化结合LQR设计横向误差与航向角误差的调节律实现精准的路径跟随。相比PID多回路调参或MPC高计算开销LQR兼顾理论清晰性与实时性特别适合中低速、无硬约束的工程场景。本文从普适的控制概念出发逐步讲解误差模型建立、Riccati方程求解、Q/R权重整定及前馈补偿策略并基于Matlab/Simulink环境完整搭建闭环仿真。文中提供的仿真模型搭建技巧、离散化处理与批量调参方法可直接迁移至相关工程项目为轨迹跟踪控制器的快速落地提供有效参考。 做车辆运动控制这段时间我把Matlab/Simulink里基于运动学模型的LQR轨迹跟踪控制算法完整跑通了一遍从模型推导到仿真验证踩了不少坑也积累了一些可以复用的经验。这套方案的核心思路是用自行车运动学模型描述车辆位姿变化在参考轨迹上定义横向误差与航向角误差再利用LQR线性二次型调节器设计状态反馈控制律最终在Simulink里搭建完整闭环仿真。这篇文章把整个思路、模型推导、控制器设计、Simulink实现以及排错过程都摊开讲适合正在做轨迹跟踪、路径跟踪相关方向或者想快速上手LQR控制算法的同学参考。1. 项目整体设计与控制思路1.1 为什么选择运动学模型而不是动力学模型做轨迹跟踪控制第一步要回答的问题就是被控对象建模到什么程度。常见的有两条路一条是运动学模型一条是动力学模型。运动学模型只关心速度与位姿之间的关系不涉及力、力矩、质量、惯量等物理参数动力学模型则需要完整的车辆参数建模复杂度高出不少。在低速场景下比如AGV小车、园区巡检车、扫地机器人速度一般在几米每秒以内轮胎的侧偏特性不明显车辆基本满足纯滚动无侧滑的几何约束这时候运动学模型已经能较好描述车辆的运动规律。换句话说在低速场景强行上动力学模型参数标定麻烦不说控制效果提升也有限属于吃力不讨好。我个人的选型原则是控制频率足够快、速度足够低、轮胎侧偏角足够小直接用运动学模型如果速度超过10m/s或者要处理极限工况下的稳定性问题再考虑引入动力学模型。这次项目场景就是典型的低速轨迹跟踪所以最终确定用运动学自行车模型作为控制器的设计基础。1.2 LQR在轨迹跟踪中的定位与优势确定模型之后接下来是控制器选型。轨迹跟踪控制常见的方案有PID、LQR、MPC模型预测控制等。PID实现简单但面对横向误差与航向角误差这两个强耦合的状态量时需要设计两套参数且缺乏系统的调参依据实际调起来很考验手感。LQR在这类问题上的优势体现在两个维度。第一它天然支持多状态变量的统一优化加权Q矩阵里写上对横向误差和航向角误差的权重R矩阵里写上对控制量的约束系统会帮你算出一个最优状态反馈增益阵K不需要像PID那样两个回路分别调参。第二LQR的代价函数同时惩罚状态误差与控制能量设计结果在线性系统中是全局最优的理论解释非常清晰。相比MPCLQR的实时性优势也十分明显。MPC每个控制周期都需要在线求解一个带约束的优化问题计算开销大LQR只要离线解一次Riccati方程得到K在线就是一个简单的矩阵乘法。对于没有硬约束需求的控制场景LQR性价比很高。这里说明一点LQR天然不处理输入饱和、状态约束这类不等式约束如果项目里有硬约束需求还是得考虑MPC或者LQR加约束管理模块。1.3 系统整体架构整个系统的控制架构可以分为四个模块参考轨迹生成、误差状态计算、LQR控制器、被控车辆模型。参考轨迹生成模块负责输出目标位置、目标航向角以及参考轨迹曲率这里可以预先规划好路径也可以实时计算。误差状态计算模块把车辆当前位姿与参考位姿作比较计算出横向误差和航向角误差。LQR控制器由状态反馈和前馈补偿两部分组成状态反馈用于把误差收敛到零前馈补偿用于消除曲率变化引起的稳态误差。被控车辆模型就是运动学方程通过积分器实现。这个架构有一个很实用的特点参考轨迹的曲率信息单独从前馈通道进入控制器不参与反馈计算这样即使轨迹的曲率变化很大反馈控制的压力也小很多跟踪效果会明显更平滑。后面我会在控制器设计章节详细展开前馈量的推导。2. 车辆运动学模型推导与离散化2.1 自行车模型推导运动学建模最常用的简化方式就是自行车模型把车辆的前后轮分别合并成一个轮子假设车辆只在平面内运动。设车辆后轴中心位置为(x, y)车身航向角为ψ轴距为L前轮转角为δ纵向速度为v。在纯滚动无侧滑假设下后轴中心的速度方向恒等于车身方向前轴中心的速度方向则偏转δ角。由几何关系可以得到车辆运动学方程$$ \begin{cases} \dot{x} v \cos\psi \ \dot{y} v \sin\psi \ \dot{\psi} \frac{v \tan\delta}{L} \end{cases} $$从方程可以看出车辆位姿的变化率由纵向速度v和前轮转角δ共同决定。横摆角速度$\dot{\psi}$与前轮转角的正切值成正比与轴距成反比这个关系在后面前馈控制量推导中还会用到。轴距越长相同的转角产生的横摆角速度越小车辆转弯越“迟钝”这一点在参数敏感性分析时要特别注意。2.2 误差模型建立轨迹跟踪的核心任务不是让车绝对位置跟踪参考位置而是让车相对参考轨迹的误差收敛。因此需要在参考轨迹上定义误差状态。常见做法是在轨迹参考点处建立Frenet坐标系参考点取与车辆当前位置匹配的最近点。设参考轨迹上匹配点的位置为(x_r, y_r)参考航向角为ψ_r车辆当前位置为(x, y)航向角为ψ。在车身坐标系下定义横向误差e_y和航向角误差e_ψ$$ e_y \cos\psi_r \cdot (y - y_r) - \sin\psi_r \cdot (x - x_r) $$$$ e_{\psi} \psi - \psi_r $$对误差状态求导结合运动学方程可以得到误差动态方程。考虑到转速变化远慢于控制频率简化处理时假设纵向速度v保持恒定经过线性化处理后得到$$ \frac{d}{dt} \begin{bmatrix} e_y \ e_{\psi} \end{bmatrix} \begin{bmatrix} 0 v \ 0 0 \end{bmatrix} \begin{bmatrix} e_y \ e_{\psi} \end{bmatrix} \begin{bmatrix} 0 \ \frac{v}{L} \end{bmatrix} \delta_e $$其中$\delta_e$是相对参考前轮转角的控制偏差量。这个线性化误差模型是LQR设计的基础。注意这里做了两个关键简化一是认为$\cos e_\psi \approx 1$二是忽略了参考曲率变化项把曲率影响单独挪到前馈通道处理。这两个简化的有效性前提是误差角较小、控制频率足够高使用时需要确认工况满足假设。2.3 离散化处理LQR设计既可以在连续域完成也可以在离散域完成。Matlab里有对应的lqr()和dlqr()函数。如果直接用连续系统的A、B矩阵求增益K然后在Simulink中以离散控制步长应用需要用零阶保持器ZOH做离散化保证理论一致。零阶保持器离散化的含义是在一个控制周期内保持控制量不变周期结束时更新一次。离散化后的状态矩阵为$$ A_d e^{A T_s}, \quad B_d \int_0^{T_s} e^{A\tau} B , d\tau $$在实际工程中可以直接用Matlab的c2d函数完成这个过程。仿真步长与控制周期需要分开理解Simulink仿真步长是求解器积分步长控制周期是控制器更新周期。若控制周期选为0.05s仿真步长设为0.001s则在每个控制周期内控制器输出保持不变车辆模型则以1ms步长连续积分。这种多速率设置更贴近真实控制系统也是后续仿真时推荐的配置。3. LQR控制器设计原理与参数整定方法3.1 LQR目标函数与控制律求解LQR问题的目标是在无限时域内最小化代价函数$$ J \int_0^{\infty} \left( x^T Q x u^T R u \right) dt $$其中x是状态向量u是控制输入Q是半正定状态加权矩阵R是正定控制加权矩阵。含义很直白让状态误差尽快收敛到零同时不让控制量过大。Q和R的对角元素分别对应不同状态分量和控制分量的惩罚权重哪个越大系统就越努力去减小它。最优控制律形式为线性状态反馈$$ u -K x $$增益矩阵K由Riccati方程的解P确定$$ A^T P P A - P B R^{-1} B^T P Q 0 $$$$ K R^{-1} B^T P $$在Matlab中直接调用K lqr(A, B, Q, R)就能完成求解不需要手写Riccati方程迭代。对离散系统则使用K dlqr(A_d, B_d, Q, R)。这里有一个常见误区dlqr传入的Q、R矩阵与连续域lqr传入的Q、R在数值含义上不完全一致离散Riccati方程的目标函数是求和形式而非积分形式参数标定在不同域之间做对比时要注意这一步。3.2 Q、R矩阵选取的工程经验Q、R的选取直接决定控制效果这也是新手最容易卡住的地方。理论教材通常只说“根据设计需求选取权重”但实际项目里这个权重怎么给需要一套可执行的思路。推荐先用Bryson规则做一个初始估计将Q的对角元素设置为状态容许最大偏差平方的倒数R设置为控制量容许最大幅值平方的倒数。$$ Q_{ii} \frac{1}{e_{i,max}^2}, \quad R \frac{1}{u_{max}^2} $$比如横向误差容许最大0.2m航向角误差容许最大0.1rad则对应的Q对角元素约为25和100前轮转角容许最大0.5rad则R取4。这里先不追求精细只需量级合理。在Bryson规则基础上再做细调。以我的实践经验在小车的轨迹跟踪场景中横向误差权重通常取1~20航向角误差权重取10~100控制权重取0.1~10。横向误差权重加大跟踪更紧但车辆转向更激进控制权重加大转向更柔和但收敛变慢。具体整定时建议按照“先保证稳定再提高精度最后优化平顺性”的顺序操作不要一上来就追求极端参数。3.3 前馈控制量推导仅靠状态反馈的LQR在曲线轨迹跟踪时必然存在稳态误差。原因是车辆过弯时需要持续保持一个基本转角这个转角由曲率决定如果只靠误差反馈来产生这个转角误差就永远无法归零。解决方法是引入前馈控制量。由运动学方程可知车辆以曲率κ的圆弧轨迹行驶时期望横摆角速度为$v\kappa$对应前轮转角为$$ \delta_{ff} \arctan(L \kappa) \approx L \kappa $$在曲率不大时近似可以取线性关系。控制器总输出为$$ \delta \delta_{ff} - K \begin{bmatrix} e_y \ e_{\psi} \end{bmatrix} $$这里有两点需要提示。第一前馈量是基于运动学关系算出来的实验过程中车辆若与模型偏差大例如轮胎打滑明显前馈量也会失效这时要么修正模型要么放弃前馈。第二前馈量的引入是纯代数计算不改变闭环系统极点分布不会影响稳定性可以放心叠加。3.4 闭环稳定性的直观理解反馈增益K确定后闭环系统矩阵变为$A - BK$其极点决定了误差收敛特性。在设计时可以观察闭环极点来判断系统响应形态。以本文的二阶误差模型为例期望极点通常配置为一对共轭复根对应二阶系统的衰减特性。用阻尼比和自然频率来直观理解阻尼比决定超调大小自然频率决定收敛快慢。Q矩阵中横向误差权重增大相当于迫使横向误差快速收敛对应自然频率增大但过大的自然频率会让控制量饱和反而引发振荡。所以参数整定时不能只看仿真曲线漂亮还要观察控制量是否在合理范围内。4. Simulink模型搭建与实现细节4.1 模型框架与模块划分Simulink模型按功能划分成参考轨迹生成、误差计算、LQR控制器、车辆运动学模型四个子系统。顶层模型连线是闭合的参考轨迹与车辆状态共同进入误差计算模块误差送入LQR控制器控制量进入车辆模型车辆模型输出状态量反馈回误差计算模块。为了便于参数修改推荐在Matlab基础工作区预先定义所有参数模型中的增益参数直接引用工作区变量。这样做的好处是批量仿真时可以通过脚本修改参数后调用sim()函数不需要手动打开模型改数值效率提升非常明显。4.2 参考轨迹生成模块参考轨迹可以有多种形式。圆形轨迹最简单只包含常数曲率适合验证基本跟踪能力正弦轨迹的曲率连续变化适合验证动态响应8字形轨迹包含曲率方向切换对控制器的抗扰动能力要求更高。这里以圆形轨迹为第一个实验对象。设圆心为(0, 0)半径R5m车辆以v2m/s速度行驶参数值圆心坐标(0, 0)半径R5 m初始位置(5, 0)初始航向角π/2纵向速度v2 m/s轴距L1.5 m参考轨迹模块需要输出参考位置(x_r, y_r)、参考航向角ψ_r以及参考曲率κ_r。在Simulink中可以用MATLAB Function模块实现代码里根据时间t实时计算目标点位置。对于圆形轨迹其他任意点处需要先找到参考匹配点再计算对应的误差信息。4.3 LQR控制器子系统LQR控制器子系统的核心是一个MATLAB Function模块输入为横向误差e_y、航向角误差e_ψ、参考曲率κ_r输出为前轮转角δ。控制器内部先计算反馈量再叠加前馈量。function delta lqr_controller(e_y, e_psi, kappa_r, K_gain, L) % 状态反馈 x_state [e_y; e_psi]; u_feedback -K_gain * x_state; % 前馈补偿 delta_ff atan(L * kappa_r); % 总控制量 delta delta_ff u_feedback; end调用lqr()求增益K时离散控制律的K需要提前在工作区算好% 运动学模型参数 L 1.5; v 2.0; Ts 0.05; % 连续系统矩阵 A [0, v; 0, 0]; B [0; v/L]; % Q、R权重 Q [5, 0; 0, 30]; R 1; % 连续LQR求解 K_cont lqr(A, B, Q, R); % 或者离散化后求解 sys_d c2d(ss(A, B, [], []), Ts, zoh); K_disc dlqr(sys_d.A, sys_d.B, Q, R);从实测效果看连续LQR求解后直接以离散周期应用只要控制周期足够短性能差别不大但如果控制周期较长超过0.1s离散化求解的必要性就体现出来了。4.4 车辆运动学子系统车辆运动学模型可以直接用积分器模块搭建也可以写S-Function。用积分器组合的方式更直观便于调试时查看中间信号。搭建方法用三个积分器分别对应x、y、ψ三个状态量积分器输入为()、(v\cos\psi)、(v\ tan\delta/L)。速度v通过常数模块给定前轮转角δ来自控制器输出。ψ需要做角度归一化处理让它保持在[-π, π]区间否则长时间仿真中角度量会越积越大虽然三角函数不受影响但误差计算时会出问题。这里要特别提到代数环问题。如果LQR控制器直接以连续积分器的输出作为反馈输入由于积分器输出到控制器输入再到积分器输入之间存在一条“输出→输入→输出”的无延迟路径Simulink会在某些环节产生代数环导致仿真报错或运行效率下降。解决办法是在反馈路径上加一个Memory模块或者把控制器模块设置为离散采样时间让它只在控制周期更新一次相当于人为制造一周期的延时从而切断代数环。4.5 仿真参数配置仿真步长设置建议如下求解器用ode4四阶龙格库塔法固定步长0.001s控制周期0.05s仿真时长20s。控制周期内控制器输出保持恒定车辆模型在每个仿真步长内连续积分这样既接近真实离散控制特性又不会因为步长过大导致积分误差累积。快速检验两个周期是否匹配的做法是把控制周期改为仿真步长的整数倍让误差在单个控制周期内可以忽略。如果控制周期远远大于仿真步长车辆模型对控制量的响应会出现台阶状误差跟踪效果会变差。5. 仿真结果与参数影响分析5.1 圆形轨迹跟踪基准结果搭建完成后先跑一组基准实验。使用如下参数Q diag([5, 30])R 1控制周期Ts 0.05s车辆初始位姿为(3, 2, π)与参考初始位姿(5, 0, π/2)存在明显偏差。仿真结果显示车辆在2~3秒内迅速将横向误差从约2m收敛到0附近并进入稳定的跟踪状态。稳态阶段的横向误差主要来自离散控制和前馈近似误差一般在厘米级。航向角误差同样收敛到接近零。这个结果说明在初始误差较大的情况下LQR依然具备很好的收敛能力。从控制量曲线看启动初期前轮转角有一个明显的瞬时冲击对应车辆快速修正初始姿态的动作随后转角平稳过渡到接近常量这个常量正是圆形轨迹所需的前馈转角。这说明反馈加前馈的组合达成了预期的分工效果。5.2 权重矩阵对跟踪效果的影响为了直观展示Q、R的作用我做了三组对比实验固定R 1改变Q矩阵。第一组Q diag([1, 10])横向误差收敛速度较慢约5秒后进入稳态但控制过程平缓前轮转角变化幅度小适合对乘坐舒适性要求高的场景。第二组Q diag([20, 50])横向误差收敛明显加快约1.5秒就进入稳态但初始阶段前轮转角迅速打到较大值控制量幅度增大对执行机构的响应速度要求更高。第三组Q diag([100, 100])出现明显的振荡趋势横向误差曲线在收敛过程中产生超调前轮转角也出现周期性波动。这说明权重过大会让系统过于激进对模型误差和离散化误差更加敏感实际控制效果反而变差。这三组实验的结论是Q矩阵不是越大越好需要结合执行器限制和模型精度综合选取。Bryson规则给的是起点后续要在它附近做局部搜索。5.3 前馈控制的效果对比单独把前馈通道去掉只保留反馈控制重复圆形轨迹跟踪实验。结果显示稳态横向误差无法收敛到零而是一个与曲率相关的常值偏置曲率越大稳态误差越大。这正是之前理论分析所说的“反馈无法消除由曲率引起的稳态误差”这个结论的直观体现。加上前馈通道后稳态误差大幅下降从厘米级降到几毫米。这里还要多说一句前馈量的准确程度取决于曲率信号是否准确。在实车或机器人平台上参考轨迹的曲率通常需要事先计算好并写入轨迹数据中不能临时差分估算否则差分噪声会被前馈通道直接放大。5.4 不同轨迹下的跟踪效果正弦轨迹的曲率连续变化控制器的前馈通道需要持续调整转角大小。在幅值1m、波长10m的正弦轨迹下跟踪效果良好相位滞后很小横向误差保持在分米级以内。8字形轨迹在曲率切换点附近横向误差略微增大但整体收敛未出现失稳现象。这些实验综合说明这套基于运动学LQR的控制方案在中低速、曲率变化平缓的轨迹上有良好的通用性。如果轨迹包含急弯曲率突变或曲率半径过小运动学模型的假设可能失效控制误差会显著增大此时需要考虑动力学模型或者低速限制。6. 常见问题与排错技巧实录6.1 仿真发散或数值爆炸症状车辆状态或控制量在仿真开始时迅速增长到无穷大Simulink报计算错误。排查顺序先看反馈增益K的符号是否反了。控制律是u -Kx如果写成u Kx正反馈会让系统瞬间发散。再检查模型连线是否正确是否把误差信号接到了控制量符号上。最后看参数是否合理如果R远小于QK矩阵会非常大控制量容易饱和甚至数值溢出。6.2 稳态误差无法消除症状跟踪圆形轨迹时横向误差稳定在一个常数不消除。原因前馈量缺失或前馈量错误。首先确认前馈通道是否生效把反馈增益K强制设为零如果车辆无法维持圆弧运动说明前馈没有正确输入。接着检查曲率计算公式曲率单位应为1/m不要把半径当成曲率使用。6.3 前轮转角抖动症状控制量在每个控制周期内大幅跳变车辆蛇形前进。原因常见于Q矩阵横向误差权重过大反馈增益过高控制频率相对模型响应速度过低。解决思路是增大R降低控制量波动幅度或者减小Q中横向误差权重使系统更平缓。也可以在控制器输出端加一阶低通滤波但滤波会引入相位延迟不能一味依赖。6.4 初始位姿与参考位姿偏差过大时收敛异常症状车辆初始位置离参考轨迹很远时仿真出现较大超调甚至冲向错误方向。原因误差模型在较大误差下线性化假设失效航向误差可能超过±π/2导致控制方向错误。解决办法是分两步先做一个粗跟踪阶段用几何方法如纯追踪法把车辆引导到参考轨迹附近再切换到LQR做精跟踪或者把航向角误差做角度归一化处理避免误差超过π。6.5 常见问题速查表现象可能原因处理建议仿真发散K符号反、正反馈检查控制律符号与连线稳态误差大前馈缺失、曲率算错单独验证前馈通道转角抖动Q过大、R过小加大R、降低Q大初始误差超调线性化失效先粗跟踪再切LQR收敛过慢Q过小、R过大提高Q中对应权重控制量饱和权重失衡限制权重幅度、加约束6.6 批量调参的实用技巧最后分享一个特别好用的调参思路不要手动在模型里改参数用脚本批量扫参。写一个循环脚本遍历多组Q、R组合调用sim()函数运行仿真把横向误差均方根、最大控制量等指标存下来最后画成表格或曲线对比。这样一次能跑几十组参数选出一组“误差小、控制量合理、过程平缓”的均衡解比肉眼反复调仿真快得多。% 批量调参示例 results []; for q_ey [1, 5, 10, 20] for q_epsi [10, 30, 50] Q_test diag([q_ey, q_epsi]); K_test lqr(A, B, Q_test, R); % 更新工作区K并运行仿真 K_gain K_test; simOut sim(trajectory_tracking_lqr.slx); % 提取误差并记录 ey_data simOut.yout{1}.Values.Data; results(end1, :) [q_ey, q_epsi, rms(ey_data), max(abs(simOut.yout{2}.Values.Data))]; end end实测下来这种参数扫描方法能覆盖绝大多数调参需求。很多人在LQR调参上花了大量时间却没找到合适参数本质上就是缺少一个系统化的搜索手段。这套基于运动学模型的LQR轨迹跟踪控制方案从模型推导到Simulink实现整体并不复杂难点主要集中在线性化模型的建立、Q/R权重的平衡以及前馈量的处理这三个环节。只要把这几个环节想清楚代码和模型本身只是时间问题。我做项目时的体会是先用最简单的圆形轨迹把闭环跑通验证控制器基本逻辑再逐步上复杂轨迹、调整权重、引入离散化设计每一步都确认无误后再进入下一步这套顺序能省掉大量排错时间。希望对正在做轨迹跟踪控制的朋友有帮助。本文还有配套的精品资源点击获取