自动驾驶横向控制：LQR与动力学模型实践-AI智能范式网

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

自动驾驶LQR控制动力学模型

|难度：普通|阅读约 8 分钟

1. 动力学跟踪误差模型与LQR控制概述

在自动驾驶横向控制领域，动力学跟踪误差模型相比传统运动学模型具有显著优势。当车速超过40km/h时，运动学模型由于忽略轮胎侧偏、载荷转移等动力学因素，控制精度会急剧下降。而动力学模型通过建立包含横向位置误差、航向角误差、横摆角速度等状态量的微分方程，能够更准确地描述车辆实际行为。

LQR（Linear Quadratic Regulator）算法通过最小化代价函数来求解最优控制律，其数学形式为：

code复制J = ∫(xᵀQx + uᵀRu)dt

其中Q矩阵权衡状态误差的重要性，R矩阵控制输入代价。通过求解Riccati方程得到最优反馈矩阵K，使控制量u=-Kx达到最优。

需要模型API调用？免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。

2. 车辆动力学建模关键步骤

2.1 状态空间方程建立

采用经典的二自由度自行车模型，状态向量选取为：

code复制x = [e_y; e_ψ; e_y'; e_ψ']

其中e_y为横向位置误差，e_ψ为航向角误差。对应的状态空间矩阵为：

matlab复制A = [0   1   0     0;
     0   0   C_αf/mv   (aC_αf-bC_αr)/mv;
     0   0   0     1; 
     0   0   (aC_αf-bC_αr)/Iz   (a²C_αf+b²C_αr)/Izv];
     
B = [0; -C_αf/m; 0; -aC_αf/Iz];

其中C_αf、C_αr分别为前后轮侧偏刚度，a、b为质心到前后轴距离，m为质量，Iz为转动惯量。

2.2 轮胎特性处理

轮胎侧偏刚度会随垂直载荷非线性变化，可采用魔术公式(Magic Formula)建模：

code复制C_α = D*sin(C*arctan(B*α - E*(B*α - arctan(B*α))))

实际工程中常采用分段线性化处理，在不同滑移角区间使用不同的等效刚度值。

3. LQR控制器实现细节

3.1 权重矩阵设计原则

Q矩阵对角元素对应状态量的权重：

横向误差权重：通常设为1-5
航向误差权重：建议0.5-2
误差微分项权重：0.1-0.5

R矩阵控制转向角速率代价，典型值为0.1-1。过小会导致控制过于激进，过大会降低响应速度。

3.2 离线计算K矩阵

matlab复制Q = diag([5, 1, 0.3, 0.2]); 
R = 0.5;
[K, ~, ~] = lqr(A, B, Q, R);

离线计算适合工况变化不大的场景，计算效率高但适应性有限。

3.3 在线迭代计算

采用实时更新的状态矩阵进行LQR求解：

matlab复制function K = onlineLQR(v, A_nom, B_nom)
    % 根据车速更新A矩阵
    A = A_nom;
    A(2,3) = A_nom(2,3)/v;
    A(4,3) = A_nom(4,3)/v;
    
    % 自适应权重调整
    Q = diag([5, 1, 0.3+0.1*v, 0.2]); 
    [K, ~, ~] = lqr(A, B_nom, Q, R);
end

在线计算能适应车速变化，但需注意数值稳定性问题。

4. 前馈补偿设计

纯反馈控制会存在稳态误差，需要加入前馈补偿：

matlab复制delta_ff = (1 + Kv*v²)*L/R;

其中Kv为不足转向梯度，L为轴距，R为路径曲率半径。前馈量可显著提高弯道跟踪精度。

5. Carsim联合仿真配置

5.1 接口配置要点

在Carsim中设置Solver为Runge-Kutta 4阶
采样时间与Simulink保持一致（建议0.01s）
输出变量需包含：
- Vehicle.Side_Slip
- Vehicle.Yaw_Rate
- Vehicle.Pos_Y
- Vehicle.Heading_Angle

5.2 参数同步问题

车辆质量、转动惯量等关键参数必须在Carsim和Simulink中保持一致。建议通过.mat文件实现参数自动同步：

matlab复制load('vehicleParams.mat');
set_param('carsim_block', 'Mass', num2str(m));

6. 不同转向模式实现

6.1 前轮转向

控制量直接映射到前轮转角：

matlab复制delta_f = -K*x;

6.2 后轮转向

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

matlab复制B = [0; -C_αr/m; 0; bC_αr/Iz];
delta_r = -K*x;

6.3 四轮转向

采用比例分配策略：

matlab复制delta_f = -Kf*x;
delta_r = -Kr*x;

其中Kf和Kr通过优化算法确定，通常后轮转角幅值为前轮的20-30%。

7. 典型问题排查指南

现象	可能原因	解决方案
低速振荡	Q矩阵中速度项权重过高	降低e_y'和e_ψ'权重
高速发散	未考虑轮胎非线性	增加速度自适应权重
弯道偏移	前馈补偿不足	调整不足转向梯度Kv
响应延迟	R矩阵值过大	逐步减小R直到0.1

8. 实际调试经验

初始调试建议从低速（30km/h）开始，逐步提高车速
先调前馈再调反馈，前馈解决稳态误差，反馈改善动态响应
在双移线工况下观察超调量，调整Q矩阵中e_ψ权重
高速工况（>80km/h）需特别注意轮胎线性区限制

通过合理配置动力学参数和LQR权重，该方案可实现0-120km/h全速域的高精度路径跟踪，横向位置误差可控制在0.1m以内。相比纯运动学控制，在高速变道等场景下稳定性提升显著。