基于动态车辆模型的百度Apollo LQR和MPC横向控制算法
一、动态车辆模型基础：单轨动力学模型
百度Apollo的横向控制算法以单轨动力学模型（又称自行车模型）为核心，该模型简化了车辆的侧向运动特性，同时保留关键动力学特性，适用于实时控制场景。
1. 车辆动力学方程
在Frenet坐标系下，考虑车辆侧向动力学，状态向量定义为：
\[x = [e_y, e_\theta, \dot{e}_y, \dot{e}_\theta]^T
\]
其中：
- \(e_y\)：横向位置误差（车辆中心与期望轨迹的偏差）
- \(e_\theta\)：航向角误差（车辆朝向与期望朝向的偏差）
- \(\dot{e}_y\)：横向速度误差
- \(\dot{e}_\theta\)：横摆角速度误差
控制输入为前轮转角：\(u = \delta_f\)
2. 线性化状态空间方程
基于小角度假设和线性轮胎模型，得到连续时间状态方程：
\[\dot{x} = A x + B u
\]
矩阵\(A\)和\(B\)取决于以下参数：
- 车速 \(v\)
- 前后轴距 \(l_f\), \(l_r\)
- 前后轮侧偏刚度 \(C_f\), \(C_r\)
- 车辆质量 \(m\)
- 横摆转动惯量 \(I_z\)
具体形式为：
\[A = \begin{bmatrix}
0 & 1 & 0 & 0 \\
0 & -\frac{2(C_f + C_r)}{mv} & \frac{2(C_f + C_r)}{m} & -\frac{2(C_f l_f - C_r l_r)}{mv} \\
0 & 0 & 0 & 1 \\
0 & -\frac{2(C_f l_f - C_r l_r)}{I_z v} & \frac{2(C_f l_f - C_r l_r)}{I_z} & -\frac{2(C_f l_f^2 + C_r l_r^2)}{I_z v}
\end{bmatrix}
\]
\[B = \begin{bmatrix}
0 \\ \frac{2C_f}{m} \\ 0 \\ \frac{2C_f l_f}{I_z}
\end{bmatrix}
\]
3. 离散化处理
采用零阶保持器（ZOH）或双线性变换将连续系统离散化：
\[x(k+1) = A_d x(k) + B_d u(k)
\]
其中\(A_d = e^{AT_s}\), \(B_d = \int_0^{T_s} e^{A\tau} B d\tau\)，\(T_s\)为采样时间。
二、Apollo LQR横向控制算法
1. 算法架构
Apollo的LQR横向控制器不是简单的教科书实现，包含多项增强特性：
┌────────────────────────────────────────────────────────┐
│ Apollo LQR 横向控制器架构 │
├────────────────────────────────────────────────────────┤
│ 1. 轨迹分析器 (TrajectoryAnalyzer) │
│ - 匹配最近轨迹点 │
│ - 计算横向误差、航向角误差 │
│ - 提取期望曲率 │
│ │
│ 2. LQR 求解器 │
│ - 构建状态空间方程 │
│ - 求解 Riccati 方程 → K 矩阵 │
│ - 计算反馈控制量: δ = -K x │
│ │
│ 3. 前馈控制 (Feedforward) │
│ - 曲率前馈: δ_ff = κ_des * (l_f + l_r - l_r*K_v*v/g)│
│ - 补偿稳态误差 │
│ │
│ 4. 预瞄控制 (Preview) │
│ - 考虑未来 N 个点的轨迹信息 │
│ - 提前响应曲线 │
│ │
│ 5. 增益调度 (Gain Scheduling) │
│ - 根据车速、曲率动态调整 Q、R │
│ - 高速 → 降低 Q,增加稳定性 │
│ - 低速 → 增加 Q,提高响应速度 │
│ │
│ 6. Lead-Lag 补偿器 │
│ - 补偿执行器延迟 │
│ - 相位超前/滞后 │
│ │
│ 7. MRAC 自适应控制 (可选) │
│ - Model Reference Adaptive Control │
│ - 在线调整控制参数 │
│ │
│ 8. 转向角限幅与速率限制 │
│ - 转向角限制:[-max_angle, max_angle] │
│ - 转向角速度限制:防止打方向过快 │
└────────────────────────────────────────────────────────┘
2. LQR问题 formulation
LQR的目标是找到最优控制律\(u(k) = -K x(k)\)，最小化二次型代价函数：
\[J = \sum_{k=0}^{\infty} [x(k)^T Q x(k) + u(k)^T R u(k)]
\]
其中\(Q\)为半正定状态权重矩阵，\(R\)为正定控制权重矩阵。
3. Riccati方程求解
通过求解离散代数Riccati方程得到反馈增益矩阵\(K\)：
\[P = A_d^T P A_d - A_d^T P B_d (R + B_d^T P B_d)^{-1} B_d^T P A_d + Q
\]
\[K = (R + B_d^T P B_d)^{-1} B_d^T P A_d
\]
4. 前馈补偿设计
为消除稳态误差，Apollo添加前馈控制项：
\[\delta_{ff} = \frac{l_f + l_r - \frac{m v^2}{2C_r(l_f+l_r)} \left( \frac{l_r}{C_f} - \frac{l_f}{C_r} \right)}{R} + K_v v^2
\]
其中\(R\)为道路曲率半径，\(K_v\)为车速补偿系数。
5. 最终控制量计算
总转向角为反馈、前馈和补偿项之和：
\[\delta = \delta_{fb} + \delta_{ff} + \delta_{comp}
\]
其中\(\delta_{fb} = -K x\)为反馈控制量，\(\delta_{comp}\)为Lead-Lag补偿器输出。
三、Apollo MPC横向控制算法
1. 算法概述
Apollo的MPC控制器用于横纵向联合控制，同时输出方向盘转角、油门、刹车指令。状态向量扩展为6维：
\[x = [e_y, \dot{e}_y, e_\theta, \dot{e}_\theta, e_s, \dot{e}_s]^T
\]
其中新增：
- \(e_s\)：纵向位置误差
- \(\dot{e}_s\)：纵向速度误差
控制向量为：
\[u = [\delta_f, a]^T
\]
其中\(a\)为加速度补偿量。
2. MPC优化问题
在预测时域\(N\)内，MPC求解以下优化问题：
\[\min_{u_0,...,u_{N-1}} \sum_{k=0}^{N-1} [x_k^T Q x_k + u_k^T R u_k] + x_N^T Q_f x_N
\]
约束条件：
- 系统动力学：\(x_{k+1} = A_d x_k + B_d u_k\)
- 控制输入约束：\(u_{min} \leq u_k \leq u_{max}\)
- 状态约束：\(x_{min} \leq x_k \leq x_{max}\)
- 控制增量约束：\(\Delta u_{min} \leq u_k - u_{k-1} \leq \Delta u_{max}\)
3. 二次规划转化
Apollo将MPC问题转化为标准二次规划形式：
\[\min_{U} \frac{1}{2} U^T H U + f^T U
\]
\[\text{s.t. } G U \leq h
\]
其中\(U = [u_0^T, ..., u_{N-1}^T]^T\)为优化变量序列。
4. 求解器实现
Apollo使用OSQP求解器进行实时优化：
apollo::common::math::MpcOsqp mpc_osqp(
matrix_ad_, matrix_bd_, matrix_q_updated_, matrix_r_updated_,
matrix_state_, lower_bound, upper_bound, lower_state_bound,
upper_state_bound, reference_state, mpc_max_iteration_, ...
);
5. 滚动优化策略
MPC采用滚动时域优化：
- 在当前时刻\(k\)，基于当前状态\(x(k)\)求解未来\(N\)步的最优控制序列\(U^* = [u^*(k), ..., u^*(k+N-1)]\)
- 仅应用第一步控制量\(u^*(k)\)
- 下一时刻\(k+1\)，重新测量状态并重复优化
四、LQR与MPC对比分析
1. 核心差异对比表
| 特性 | LQR (线性二次调节器) | MPC (模型预测控制) |
|---|---|---|
| 控制维度 | 横向控制 (2D/4D) | 横纵向联合控制 (6D) |
| 工作时域 | 无限时域/固定时域 | 有限滚动时域 |
| 优化方式 | 离线一次求解 | 在线滚动优化 |
| 约束处理 | 无显式约束，需后验调整 | 显式处理硬约束 |
| 模型要求 | 线性模型 | 可处理非线性模型 |
| 计算复杂度 | 低，解析解 | 高，在线优化 |
| 实时性 | 高，适合高速场景 | 中等，适合复杂场景 |
| 应用场景 | 高速公路、结构化道路 | 复杂场景、停车、低速 |
2. 理论差异详解
- 工作时域：LQR在固定时域上求解，一个时域内只有一个最优解；MPC在逐渐消减的时域内求解，最优解经常更新。
- 约束处理：LQR假设控制量不受约束，实际需后验限幅；MPC显式考虑控制量和状态约束。
- 非线性处理：LQR依赖线性模型；MPC可处理非线性系统偏离线性化工作点的迁移。
3. Apollo中的算法选择策略
Apollo根据场景动态选择控制算法：
开始
↓
需要综合控制吗？
/ \
是 否
↓ ↓
使用MPC 横向还是纵向？
(parking/低速) / \
横向 纵向
↓ ↓
使用LQR 使用PID
(高速/曲线) (速度控制)
典型配置组合：
- 标准组合：LQR横向控制 + PID纵向控制（高速场景）
- 复杂场景：MPC横纵向联合控制（低速、停车、弯道）
- 自适应切换：根据车速、曲率、跟踪误差动态切换
参考代码 论文+程序 基于动态车辆模型的 Baidu Apollo lqr和mpc横向控制算法 www.youwenfan.com/contentcnt/160712.html
五、MATLAB/Simulink实现示例
1. LQR控制器实现
function [delta, K] = lqr_lateral_control(v, ey, etheta, ey_dot, etheta_dot, params)
% LQR横向控制器实现
% 输入：v-车速(m/s)，ey-横向误差(m)，etheta-航向误差(rad)
% ey_dot-横向误差变化率，etheta_dot-航向误差变化率
% params-车辆参数结构体
% 输出：delta-前轮转角(rad)，K-LQR反馈增益矩阵
% 提取参数
m = params.m; % 质量(kg)
Iz = params.Iz; % 横摆转动惯量(kg·m²)
lf = params.lf; % 前轴距(m)
lr = params.lr; % 后轴距(m)
Cf = params.Cf; % 前轮侧偏刚度(N/rad)
Cr = params.Cr; % 后轮侧偏刚度(N/rad)
Ts = params.Ts; % 采样时间(s)
% 构建连续时间状态矩阵A, B
A = zeros(4,4);
B = zeros(4,1);
% A矩阵
A(1,2) = 1;
A(2,2) = -(2*(Cf+Cr))/(m*v);
A(2,3) = 2*(Cf+Cr)/m;
A(2,4) = -(2*(Cf*lf-Cr*lr))/(m*v);
A(3,4) = 1;
A(4,2) = -(2*(Cf*lf-Cr*lr))/(Iz*v);
A(4,3) = 2*(Cf*lf-Cr*lr)/Iz;
A(4,4) = -(2*(Cf*lf^2+Cr*lr^2))/(Iz*v);
% B矩阵
B(2) = 2*Cf/m;
B(4) = 2*Cf*lf/Iz;
% 离散化（零阶保持）
sysc = ss(A, B, eye(4), 0);
sysd = c2d(sysc, Ts, 'zoh');
Ad = sysd.A;
Bd = sysd.B;
% LQR权重矩阵（需根据车速调整）
Q = diag([10, 0.1, 5, 0.1]); % 状态权重
R = 0.1; % 控制权重
% 求解Riccati方程
[K, ~, ~] = dlqr(Ad, Bd, Q, R);
% 状态向量
x = [ey; ey_dot; etheta; etheta_dot];
% 反馈控制量
delta_fb = -K * x;
% 前馈控制量（道路曲率补偿）
curvature = params.curvature; % 道路曲率(1/m)
delta_ff = (lf + lr) * curvature + ...
(m * v^2 / (2 * Cr * (lf+lr))) * (lr/Cf - lf/Cr) * curvature;
% 总控制量
delta = delta_fb + delta_ff;
% 转向角限幅
delta_max = deg2rad(30); % 最大转向角±30°
delta = max(min(delta, delta_max), -delta_max);
end
2. MPC控制器实现
function [u_opt, cost] = mpc_lateral_control(x0, ref_traj, params)
% MPC横向控制器实现
% 输入：x0-初始状态[ey; ey_dot; etheta; etheta_dot]
% ref_traj-参考轨迹[N×4]
% params-控制器参数
% 输出：u_opt-最优控制序列，cost-优化代价
% 参数设置
N = params.N; % 预测时域
Q = params.Q; % 状态权重矩阵
R = params.R; % 控制权重矩阵
Ts = params.Ts; % 采样时间
% 车辆动力学模型（离散化）
[Ad, Bd] = get_vehicle_model(params);
% 定义优化变量
X = sdpvar(4, N+1); % 状态序列
U = sdpvar(1, N); % 控制序列
% 初始状态约束
constraints = [X(:,1) == x0];
% 动力学约束
for k = 1:N
constraints = [constraints,
X(:,k+1) == Ad*X(:,k) + Bd*U(:,k)];
end
% 控制输入约束
delta_max = deg2rad(30);
constraints = [constraints, -delta_max <= U <= delta_max];
% 控制增量约束
if isfield(params, 'delta_u_max')
for k = 2:N
constraints = [constraints,
-params.delta_u_max <= U(k)-U(k-1) <= params.delta_u_max];
end
end
% 代价函数
cost = 0;
for k = 1:N
state_err = X(:,k) - ref_traj(k,:)';
cost = cost + state_err'*Q*state_err + U(k)'*R*U(k);
end
% 终端代价
terminal_err = X(:,N+1) - ref_traj(N+1,:)';
cost = cost + terminal_err'*params.Qf*terminal_err;
% 求解优化问题
options = sdpsettings('solver', 'osqp', 'verbose', 0);
sol = optimize(constraints, cost, options);
if sol.problem == 0
u_opt = value(U);
cost = value(cost);
else
u_opt = zeros(1, N);
cost = inf;
end
end
function [Ad, Bd] = get_vehicle_model(params)
% 获取离散车辆模型
v = params.v;
m = params.m;
Iz = params.Iz;
lf = params.lf;
lr = params.lr;
Cf = params.Cf;
Cr = params.Cr;
Ts = params.Ts;
% 连续时间矩阵
A = [0, 1, 0, 0;
0, -(2*(Cf+Cr))/(m*v), 2*(Cf+Cr)/m, -(2*(Cf*lf-Cr*lr))/(m*v);
0, 0, 0, 1;
0, -(2*(Cf*lf-Cr*lr))/(Iz*v), 2*(Cf*lf-Cr*lr)/Iz, -(2*(Cf*lf^2+Cr*lr^2))/(Iz*v)];
B = [0; 2*Cf/m; 0; 2*Cf*lf/Iz];
% 离散化
sysc = ss(A, B, eye(4), 0);
sysd = c2d(sysc, Ts, 'zoh');
Ad = sysd.A;
Bd = sysd.B;
end
3. 主仿真程序
% 主仿真：对比LQR和MPC性能
clear; clc; close all;
% 车辆参数
params.m = 1500; % 质量(kg)
params.Iz = 2500; % 横摆转动惯量(kg·m²)
params.lf = 1.2; % 前轴距(m)
params.lr = 1.6; % 后轴距(m)
params.Cf = 80000; % 前轮侧偏刚度(N/rad)
params.Cr = 100000; % 后轮侧偏刚度(N/rad)
params.Ts = 0.02; % 采样时间(s)
params.v = 15; % 车速(m/s)
% 参考轨迹（双移线）
t = 0:params.Ts:10;
ref_traj = zeros(length(t), 4);
for i = 1:length(t)
if t(i) < 2
ref_traj(i,1) = 0;
elseif t(i) < 6
ref_traj(i,1) = 3 * sin(pi*(t(i)-2)/4);
else
ref_traj(i,1) = 0;
end
ref_traj(i,3) = atan2(gradient(ref_traj(:,1), params.Ts), params.v);
end
% 初始化
x_lqr = [0; 0; 0; 0];
x_mpc = [0; 0; 0; 0];
u_lqr = [];
u_mpc = [];
track_err_lqr = [];
track_err_mpc = [];
% 仿真循环
for k = 1:length(t)-1
% LQR控制
[delta_lqr, K] = lqr_lateral_control(params.v, x_lqr(1), x_lqr(3), ...
x_lqr(2), x_lqr(4), params);
% MPC控制
mpc_params = params;
mpc_params.N = 10;
mpc_params.Q = diag([100, 1, 50, 1]);
mpc_params.R = 0.5;
mpc_params.Qf = diag([200, 2, 100, 2]);
[U_mpc, ~] = mpc_lateral_control(x_mpc, ref_traj(k:min(k+mpc_params.N, end), :), mpc_params);
delta_mpc = U_mpc(1);
% 更新状态（简化车辆模型）
x_lqr = update_vehicle_state(x_lqr, delta_lqr, params);
x_mpc = update_vehicle_state(x_mpc, delta_mpc, params);
% 记录数据
u_lqr = [u_lqr; delta_lqr];
u_mpc = [u_mpc; delta_mpc];
track_err_lqr = [track_err_lqr; x_lqr(1) - ref_traj(k,1)];
track_err_mpc = [track_err_mpc; x_mpc(1) - ref_traj(k,1)];
end
% 性能分析
fprintf('LQR性能指标：\n');
fprintf(' 最大横向误差：%.4f m\n', max(abs(track_err_lqr)));
fprintf(' RMS横向误差：%.4f m\n', rms(track_err_lqr));
fprintf(' 最大转向角：%.2f deg\n', rad2deg(max(abs(u_lqr))));
fprintf('\nMPC性能指标：\n');
fprintf(' 最大横向误差：%.4f m\n', max(abs(track_err_mpc)));
fprintf(' RMS横向误差：%.4f m\n', rms(track_err_mpc));
fprintf(' 最大转向角：%.2f deg\n', rad2deg(max(abs(u_mpc))));
% 可视化
figure('Position', [100, 100, 1200, 800]);
subplot(3,2,1);
plot(t(1:end-1), track_err_lqr, 'b-', 'LineWidth', 1.5); hold on;
plot(t(1:end-1), track_err_mpc, 'r--', 'LineWidth', 1.5);
xlabel('时间(s)'); ylabel('横向误差(m)');
title('横向跟踪误差对比');
legend('LQR', 'MPC'); grid on;
subplot(3,2,2);
plot(t(1:end-1), rad2deg(u_lqr), 'b-', 'LineWidth', 1.5); hold on;
plot(t(1:end-1), rad2deg(u_mpc), 'r--', 'LineWidth', 1.5);
xlabel('时间(s)'); ylabel('前轮转角(deg)');
title('控制输入对比');
legend('LQR', 'MPC'); grid on;
subplot(3,2,3);
plot(params.v * t(1:end-1), ref_traj(1:end-1,1), 'k-', 'LineWidth', 2); hold on;
plot(params.v * t(1:end-1), params.v * t(1:end-1) * 0 + track_err_lqr, 'b-', 'LineWidth', 1.5);
xlabel('纵向位置(m)'); ylabel('横向位置(m)');
title('LQR跟踪轨迹'); grid on;
subplot(3,2,4);
plot(params.v * t(1:end-1), ref_traj(1:end-1,1), 'k-', 'LineWidth', 2); hold on;
plot(params.v * t(1:end-1), params.v * t(1:end-1) * 0 + track_err_mpc, 'r--', 'LineWidth', 1.5);
xlabel('纵向位置(m)'); ylabel('横向位置(m)');
title('MPC跟踪轨迹'); grid on;
subplot(3,2,5);
histogram(track_err_lqr, 30, 'FaceColor', 'b', 'EdgeColor', 'none');
xlabel('横向误差(m)'); ylabel('频次');
title('LQR误差分布'); grid on;
subplot(3,2,6);
histogram(track_err_mpc, 30, 'FaceColor', 'r', 'EdgeColor', 'none');
xlabel('横向误差(m)'); ylabel('频次');
title('MPC误差分布'); grid on;
六、工程实践建议
1. 参数整定
LQR参数整定：
- 将\(Q\)矩阵所有元素初始化为零
- 增加\(Q(3,3)\)（航向误差权重）以最小化航向误差
- 增加\(Q(1,1)\)（横向误差权重）以最小化横向误差
- 根据车速动态调整：高速时降低\(Q\)权重增加稳定性，低速时增加\(Q\)权重提高响应速度
MPC参数整定：
- 预测时域\(N\)：通常10-20步，对应2-4秒
- 控制时域\(M\)：通常\(M \leq N/2\)
- 权重矩阵：\(Q\)比\(R\)大1-2个数量级，确保跟踪精度
2. 实时性优化
- 模型简化：使用线性化模型或降阶模型
- 求解器选择：OSQP、qpOASES等高效QP求解器
- 热启动：利用上一周期解作为初始猜测
- 并行计算：上下层控制器独立线程运行
3. 鲁棒性增强
- 参数自适应：根据车速、路面附着系数在线调整模型参数
- 扰动观测器：估计并补偿未建模动态和外部扰动
- 故障检测：监控执行器故障和传感器异常
七、总结
百度Apollo的横向控制算法基于动态车辆模型，提供了LQR和MPC两种解决方案：
- LQR控制器：理论成熟、计算高效，适合高速结构化道路场景，通过前馈补偿和增益调度增强性能。
- MPC控制器：处理约束能力强，支持横纵向联合控制，适合复杂低速场景，通过滚动优化实现自适应控制。
浙公网安备 33010602011771号