自动驾驶横向控制：动力学模型与LQR算法实践

1. 动力学跟踪误差模型LQR算法概述

在自动驾驶横向控制领域，动力学跟踪误差模型结合LQR（线性二次型调节器）算法已经成为工业界的主流解决方案之一。这套方法最早由百度Apollo团队在2017年公开，经过多年迭代已经发展出多个变种版本。与传统的运动学模型相比，动力学模型最大的优势在于考虑了轮胎侧偏特性、载荷转移等实际物理因素，使得控制器在高速工况下（通常指车速>60km/h）仍能保持稳定的跟踪性能。

我在实际工程验证中发现，基于运动学模型的控制器在车速超过40km/h时，跟踪误差会呈指数级增长。而采用动力学模型后，即使在120km/h的高速工况下，双移线跟踪的横向误差也能控制在0.1米以内。这种性能提升主要来自三个方面：

1. 模型引入了轮胎侧向力与侧偏角之间的非线性关系
2. 考虑了横摆力矩平衡对车辆动态的影响
3. 通过前馈补偿消除了稳态跟踪误差

2. 车辆动力学建模关键点

2.1 二自由度单车模型

建立准确的动力学模型是LQR控制器设计的基础。最常用的简化模型是二自由度(2-DOF)单车模型，其核心假设包括：

• 忽略悬架运动，车辆简化为刚性体
• 左右轮胎特性合并处理
• 纵向动力学与横向动力学解耦

模型的状态方程可以表示为：

matlab复制% 状态变量: [横向误差, 横向误差率, 航向误差, 航向误差率]
A = [0, 1, 0, 0;
     0, -(Cf+Cr)/(m*Vx), (Cf+Cr)/m, (a*Cf-b*Cr)/(m*Vx);
     0, 0, 0, 1;
     0, (b*Cr-a*Cf)/(Iz*Vx), (a*Cf-b*Cr)/Iz, -(a^2*Cf+b^2*Cr)/(Iz*Vx)];

B = [0; Cf/m; 0; a*Cf/Iz];  % 前轮转向输入

其中Cf/Cr为前后轮侧偏刚度，a/b为质心到前后轴距离，m为质量，Iz为横摆转动惯量，Vx为纵向车速。

实际工程中需要注意：当车速Vx趋近于零时，矩阵元素会出现奇异值。因此需要设置最小车速阈值（通常取0.5m/s），低于该阈值时切换为运动学控制。

2.2 轮胎非线性特性处理

轮胎侧偏特性是动力学模型中最关键的非线性因素。在小侧偏角范围内（<5°），侧向力与侧偏角呈线性关系：

math复制Fy = Cα * α

但在大侧偏角工况下，必须采用更复杂的轮胎模型（如Pacejka魔术公式）：

matlab复制% Pacejka魔术公式示例
D = μ*Fz;
BCD = C*D;
α_slide = atan(3*μ*Fz/C); 
Fy = D*sin(C*atan(B*α - E*(B*α - atan(B*α))));

在控制器设计中，通常采用线性化处理配合增益调度（Gain Scheduling）来平衡计算复杂度与控制精度。我的经验是：在车速>80km/h时，轮胎非线性特性对控制性能的影响会显著增大，此时需要采用多组局部线性模型进行切换。

3. LQR控制器设计与实现

3.1 权重矩阵设计原则

LQR性能指标函数为：

math复制J = ∫(x'Qx + u'Ru)dt

Q和R矩阵的设计直接影响控制效果。经过多次实车测试验证，我总结出以下设计准则：

1. 横向误差权重：通常设为1，作为基准参考值
2. 航向误差权重：建议取0.5-0.8，过大会导致转向抖动
3. 误差率项权重：取0.1-0.3，用于抑制高频振荡
4. 控制量权重R：与车速成反比，高速时取较小值（0.1-0.5）

具体实现示例：

matlab复制Q = diag([1.0, 0.2, 0.7, 0.1]); 
R = 0.3 / (Vx + 0.5);  % 防止除零
[K,~,~] = lqr(A, B, Q, R);

3.2 前馈补偿设计

纯反馈控制无法消除稳态误差，必须引入前馈控制量。前馈转向角计算公式为：

math复制δ_ff = (1 + Kv*Vx^2) * L/R

其中Kv为不足转向梯度，L为轴距，R为路径曲率半径。

在实际应用中需要注意：

• 当路径曲率突变时，需要对前馈量进行低通滤波（截止频率2-5Hz）
• 在湿滑路面需要根据摩擦系数μ自适应调整Kv值

4. 联合仿真实现技巧

4.1 CarSim-MATLAB接口配置

CarSim与Simulink的联合仿真需要特别注意以下配置项：

1. 采样时间同步：建议设置为0.01s（100Hz）
2. 信号单位转换：CarSim使用度而非弧度表示角度
3. 初始状态对齐：确保仿真开始时车辆位置与路径起点匹配

典型的接口配置代码：

matlab复制% CarSim S-Function参数设置
cs_port = 10001;  % UDP端口号
cs_ip = '127.0.0.1'; 
vs_model = 'B-Class';  % 车辆模型
sim('carsim_co_sim.slx');

4.2 典型工况测试结果

在双移线工况下的性能对比（车速80km/h）：

5. 工程实践中的关键问题

5.1 时滞补偿方法

实际系统存在约100-200ms的执行时滞，会导致相位滞后。有效的补偿方案包括：

1. Smith预估器：提前计算未来状态
2. 状态预测：基于当前状态微分进行外推
3. 时滞观测器：构建时滞模型进行补偿

实测表明，采用二阶泰勒展开的状态预测效果最佳：

matlab复制x_pred = x + dx*T_delay + 0.5*ddx*T_delay^2;

5.2 不同转向模式的实现

5.2.1 后轮转向控制

需要修改B矩阵和控制分配：

matlab复制B_rws = [0; Cr/m; 0; -b*Cr/Iz];  % 后轮转向
u_rws = -K_rws * x;

5.2.2 四轮转向控制

采用控制分配矩阵实现：

matlab复制M = [Cf,  Cf;
     a*Cf, -b*Cr];  % 控制有效性矩阵
F_des = [Fy_des; Mz_des];  % 期望力/力矩
u_4ws = pinv(M) * F_des;  % 伪逆求解

6. 参数敏感性分析

通过蒙特卡洛仿真发现，对控制性能影响最大的三个参数及其容许偏差范围：

建议在实际部署时：

1. 每6个月进行一次轮胎参数标定
2. 当载重变化超过30%时重新计算控制器参数
3. 采用在线参数估计技术实时更新关键参数