EYES 乱1. 项目概述：基于MPC的无人驾驶车辆轨迹跟踪控制
在自动驾驶领域，让车辆精准跟踪预定轨迹是核心挑战之一。模型预测控制（MPC）因其出色的多变量处理能力和约束处理能力，已成为解决这一问题的首选方案。本文将详细解析如何从零构建一个完整的MPC轨迹跟踪控制器，包括动力学建模、控制器设计、优化求解以及实际调参技巧。
与传统的PID控制相比，MPC的最大优势在于其"预见性"。它通过求解一个有限时域内的优化问题，不仅考虑当前状态，还预测未来一段时间内的系统行为。这种特性使其特别适合处理具有时滞、约束和非线性特性的车辆控制系统。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 车辆动力学建模
2.1 二自由度自行车模型
车辆动力学模型是MPC控制器设计的基础。我们采用经典的二自由度自行车模型，它在保证足够精度的同时，计算复杂度适中，适合实时控制。
模型的核心状态变量包括：
- 横向位移 y
- 横摆角 φ
- 纵向速度 vx
- 横向速度 vy
- 横摆角速度 ω
模型的动力学方程可表示为：
code复制ẋ = vx*cos(φ) - vy*sin(φ)
ẏ = vx*sin(φ) + vy*cos(φ)
φ̇ = ω
v̇x = ax
v̇y = (Fyf + Fyr)/m - vx*ω
ω̇ = (a*Fyf - b*Fyr)/Iz
其中，Fyf和Fyr分别为前后轮的侧向力，可通过轮胎魔术公式计算：
code复制Fyf = Df*sin(Cf*arctan(Bf*αf))
Fyr = Dr*sin(Cr*arctan(Br*αr))
2.2 轮胎侧偏角计算
轮胎侧偏角是影响车辆动力学响应的关键参数。前轮侧偏角αf和后轮侧偏角αr的计算公式为：
code复制αf = δ - arctan((vy + a*ω)/vx)
αr = -arctan((vy - b*ω)/vx)
在实际应用中，我们通常需要对模型进行线性化处理，以便于MPC控制器的设计。常用的方法是在工作点附近进行泰勒展开，得到线性时变系统。
3. MPC控制器设计
3.1 预测模型构建
MPC的核心是构建预测模型。我们将连续时间的车辆模型离散化，常用的方法有前向欧拉法：
code复制x(k+1) = x(k) + dt*f(x(k), u(k))
其中dt为采样时间。对于我们的二自由度模型，离散化后的状态空间方程为：
code复制x(k+1) = Ad*x(k) + Bd*u(k)
y(k) = Cd*x(k)
雅可比矩阵Ad和Bd的计算需要考虑当前工作点的线性化。在实际实现中，我们通常在每个控制周期重新计算这些矩阵，以处理系统的非线性特性。
3.2 优化问题构建
MPC通过求解如下优化问题来计算最优控制输入：
code复制min Σ [x'(k)Qx(k) + u'(k)Ru(k)] + x'(N)Px(N)
s.t. x(k+1) = Ad*x(k) + Bd*u(k)
u_min ≤ u(k) ≤ u_max
Δu_min ≤ Δu(k) ≤ Δu_max
y_min ≤ y(k) ≤ y_max
其中：
- Q为状态权重矩阵
- R为控制输入权重矩阵
- P为终端代价矩阵
- N为预测时域
3.3 约束处理
实际车辆控制中存在多种约束，必须妥善处理：
-
执行器约束：
- 方向盘转角限制：通常±30度
- 加速度限制：通常±3 m/s²
-
动力学约束：
- 摩擦圆约束：确保轮胎力不超过最大附着极限
- 侧偏角约束：防止轮胎进入非线性区域
-
舒适性约束：
- 转向角速度限制
- 加加速度(jerk)限制
这些约束可以转化为QP问题的线性不等式约束，确保解的可行性。
4. 参考轨迹生成
4.1 双移线轨迹
双移线是评估车辆操控性能的经典轨迹。其数学表达式为：
code复制y_ref(x) =
d/2*(1 + tanh(z1)) - d/2*(1 + tanh(z2))
其中：
z1 = (x - x1)/a
z2 = (x - x2)/a
参数d控制车道宽度，a控制过渡区长度，x1和x2控制换道位置。
4.2 五次多项式轨迹
五次多项式轨迹可以提供平滑的位置、速度和加速度变化：
code复制y_ref(s) = a0 + a1*s + a2*s² + a3*s³ + a4*s⁴ + a5*s⁵
系数a0-a5通过边界条件确定，包括起止点的位置、速度和加速度。
在实际应用中，参考轨迹的曲率及其导数必须连续，否则会导致控制输入剧烈波动。我们通常会在轨迹生成层进行二阶导数平滑处理。
5. 求解器实现与优化
5.1 QP问题构建
将MPC问题转化为标准QP形式：
code复制min 1/2 z'Hz + f'z
s.t. lb ≤ Az ≤ ub
其中：
- z = [u(0), ..., u(N-1), x(1), ..., x(N)]'
- H为Hessian矩阵
- f为线性项
- A为约束矩阵
5.2 求解器选择
常用的QP求解器包括：
- OSQP：基于ADMM算法，适合嵌入式系统
- qpOASES：适合小规模问题，实时性好
- IPOPT：处理非线性约束能力强
对于实时性要求高的应用，OSQP是较好的选择。其典型求解时间在10ms以内，能满足100Hz的控制频率要求。
5.3 热启动技术
利用上一时刻的解作为当前优化的初始猜测，可以显著减少迭代次数。实际测试表明，热启动可将求解时间缩短30-50%。
6. 参数调试与性能优化
6.1 权重参数调节
Q和R矩阵的选择直接影响控制性能：
- 增大Q中的横向误差权重：提高跟踪精度，但可能导致控制输入波动
- 增大R中的转向权重：平滑转向输入，但可能降低响应速度
建议调试流程：
- 先调大Q，确保基本跟踪性能
- 然后调节R，平滑控制输入
- 最后微调终端代价P，改善稳定性
6.2 预测时域选择
预测时域Tp = N*dt的影响：
- 过短：预见性不足，容易失稳
- 过长：计算负担重，实时性差
经验法则：Tp应覆盖车辆的主要动态响应时间，通常1-3秒为宜。对于高速场景(>60km/h)，建议Tp≥2秒。
6.3 采样时间选择
采样时间dt的权衡：
- 过大：离散化误差显著，控制粗糙
- 过小：计算负担增加，可能无法实时求解
通常选择dt在0.05-0.2秒之间。对于动态变化快的场景，建议dt≤0.1秒。
7. 实际应用中的挑战与解决方案
7.1 模型-实际差异处理
车辆模型总是存在简化，导致模型与实际行为不一致。解决方法包括：
- 增加模型不确定性边界
- 设计鲁棒MPC控制器
- 在线参数估计
7.2 计算资源限制
嵌入式平台计算能力有限，可采用的优化手段：
- 降低预测时域
- 简化车辆模型
- 使用定点数运算
- 代码优化（循环展开、SIMD指令等）
7.3 传感器噪声影响
测量噪声会降低控制性能，应对措施：
- 设计状态观测器（如卡尔曼滤波）
- 在MPC中考虑噪声特性
- 增加输入滤波
8. 实测性能评估
在双移线测试场景下（车速60km/h），典型性能指标：
- 最大横向误差：<0.2m
- 转向角速度：<30deg/s
- 计算时间：<15ms/步
在五次多项式轨迹跟踪中，需要特别注意曲率变化率。实测表明，当曲率变化率超过0.1m⁻²/s时，需要特别调整MPC参数以避免控制输入饱和。
9. 进阶优化方向
9.1 非线性MPC
对于极限工况，线性MPC可能不足，可考虑：
- 非线性MPC
- 多模型MPC
- 自适应MPC
9.2 学习增强MPC
结合机器学习方法：
- 使用NN学习模型误差
- 强化学习优化MPC参数
- 数据驱动约束建模
9.3 硬件在环测试
在实际部署前，建议进行：
- CarSim/Prescan仿真
- 硬件在环测试
- 实车渐进式测试
在调试过程中，我发现参考轨迹的平滑性比形状本身更重要。一次实验中，轨迹上的一个小曲率突变导致MPC输出剧烈振荡。后来在轨迹生成层添加了二阶导数平滑后，控制质量显著提升。另一个实用技巧是在QP求解失败时（如由于数值问题），可以回退到上一时刻的解或切换到备用控制器，这在实际部署中至关重要。