LQR横向轨迹跟踪控制，利用simulink和carsim联合仿真，图为在双移线状况下的仿真效果。 版本各为2018和2019 如图跟踪效果较好
最近在搞车辆横向控制，试了LQR（线性二次调节器）算法配合CarSim和Simulink联合仿真，效果居然比预想的稳。特别是双移线这种考验车辆瞬态响应的工况，方向盘转角控制得像老司机——不慌不忙，轨迹贴合得漂亮。这里分享下实现过程，顺带聊聊代码里那些“灵魂参数”。
先唠唠LQR的“核心套路”
LQR说白了就是个“精打细算”的控制器。它的目标是让系统状态尽快收敛到参考轨迹，同时控制量别太猛（省轮胎还省电）。数学上就是最小化一个二次型代价函数：
J = ∫(x^T Q x + u^T R u) dt
其中，Q和R这两个矩阵决定了“状态偏差”和“控制消耗”的权重。调参的奥义全在这俩矩阵的博弈里。
举个栗子，假设车辆横向误差突然变大，Q矩阵会让控制器优先修正位置，而R矩阵则会限制方向盘打得太急。实际调试时，我习惯先把Q对角线上的横向误差权重调高10倍，R保持较小值，观察车辆是否“舍得打方向”，再逐步微调。
Simulink和CarSim的“握手协议”
CarSim负责提供高精度车辆模型，Simulink跑控制算法，两者通过S-Function交换数据。版本用2018和2019基本没坑，注意CarSim输出接口的变量名别写错就行。
联合仿真配置的关键代码段长这样：
% Carsim初始化设置 vsb = vsbCOM(); vsb.disconnect(); vsb.setup('Carsim_Model_Path.sim'); vsb.load(); % 设定仿真步长 vsb.set_param('SIM_STEP', 0.001);这里有个小坑：CarSim的仿真步长最好和Simulink保持一致，否则数据异步可能导致方向盘“抽风”。实测0.001秒步长足够稳定，再大就开始抖了。
状态方程里的“车辆语言”
LQR需要状态空间模型，这里选了经典的自行车模型，状态量包括横向误差、航向角误差、横摆角速度。代码里用到的状态矩阵A和B是这样算的：
% 车辆参数（来自CarSim） m = 1723; % 质量 lf = 1.232; lr = 1.468; Caf = 80000; Car = 80000; % 状态矩阵A A = [0, 1, 0, 0; 0, -(2*Caf + 2*Car)/(m*Vx), (2*Caf + 2*Car)/m, (-2*Caf*lf + 2*Car*lr)/(m*Vx); 0, 0, 0, 1; 0, (-2*Caf*lf + 2*Car*lr)/(Iz*Vx), (2*Caf*lf - 2*Car*lr)/Iz, -(2*Caf*lf^2 + 2*Car*lr^2)/(Iz*Vx)]; % 控制矩阵B B = [0; 2*Caf/m; 0; 2*Caf*lf/Iz];注意车速Vx是时变的，这里做了准静态假设——每次迭代更新Vx的值。虽然简化了模型，但实测在双移线60km/h下够用，再高就得考虑动力学非线性了。
Riccati方程求解：MATLAB的“作弊器”
计算反馈增益矩阵K时，直接调用lqr函数省事：
[K, S, e] = lqr(A, B, Q, R);但别以为这就完了！关键在Q和R的初始化策略。我的经验是先用对角矩阵试水：
Q = diag([10, 0.1, 5, 0.01]); % 横向误差权重最高 R = 0.01; % 允许适当的方向盘转动如果发现车辆“画龙”，可能是航向角误差权重不够；要是方向盘抖动，把R调大压一压。
双移线仿真：“蛇形走位”的考验
设置双移线参考轨迹时，CarSim的Path模块可以直接导入坐标点。仿真结果如图所示，横向误差基本控制在0.1米内（毕竟不是模型预测控制，这精度已经感人）。
方向盘转角曲线平滑得像手绘——没有高频抖振，说明R矩阵起了作用。有趣的是，在第二个弯道切换时，LQR会提前0.5秒开始打方向，有种“预判”的感觉，其实是状态量中的横摆角速度起了前瞻作用。
说点人话：别死磕理论
调LQR最大的误区就是沉迷推导公式。实际工程中，模型误差比算法本身影响更大。比如CarSim的轮胎松弛特性没考虑的话，再牛的控制律也救不回来。建议先验证模型准确性，再怼参数优化。
代码虽简单，但魔鬼在细节：
- 车辆参数务必和CarSim模型对齐，尤其是轮胎侧偏刚度；
- 仿真步长别设太大，否则离散化误差会让理论派哭晕；
- 双移线速度别作死上100km/h，自行车模型扛不住。
最后扔个彩蛋：把Q矩阵的横向误差权重调到100，车辆会变成“强迫症”——宁可颠簸也要贴紧路径，适合给自动驾驶的“完美主义者”参考。（笑）
