无人驾驶车辆MPC控制与动力学建模实践

1. 无人驾驶车辆MPC控制的核心逻辑

在无人驾驶车辆控制领域，模型预测控制（MPC）之所以成为主流方案，核心在于其独特的"预判-优化-执行"闭环机制。与传统PID控制不同，MPC在每个控制周期都会求解一个有限时域内的优化问题，这使得系统能够提前规避潜在风险。

以双移线轨迹跟踪为例，MPC控制器的工作流程可以分解为三个关键阶段：

1. 预测阶段：基于当前状态和动力学模型，预测未来N步的系统行为
2. 优化阶段：求解使预测轨迹与参考轨迹误差最小的控制序列
3. 执行阶段：仅应用优化结果的第一步控制量，然后进入下一个周期

这种滚动时域优化的特性，使得MPC天然具备处理系统约束的能力。在实际车辆控制中，我们需要考虑的约束包括但不限于：

• 执行器物理限制（方向盘转角±30度）
• 轮胎摩擦圆约束（侧向加速度≤0.8g）
• 动力学可行性（横摆角速度连续可导）

2. 车辆动力学建模要点解析

2.1 二自由度自行车模型构建

车辆动力学建模是MPC控制的基础，其中二自由度自行车模型因其简洁有效而被广泛采用。该模型将四轮车辆简化为前后两个轮，主要描述横向和横摆运动：

code复制def bicycle_model(x, u, dt):
    # 状态变量x=[y, φ, vx, vy, r]
    # 控制输入u=[δ, a]
    beta = np.arctan(lr * np.tan(u[0]) / (lf + lr))  # 等效侧偏角
    dxdt = [
        x[3]*np.cos(x[1]) + x[2]*np.sin(x[1]),  # 横向位移微分
        x[4],                                  # 横摆角微分 
        u[1],                                  # 纵向加速度
        (Fyf*np.cos(u[0]) + Fyr)/m - x[4]*x[2], # 横向加速度
        (lf*Fyf - lr*Fyr)/Iz                    # 横摆角加速度
    ]
    return x + np.array(dxdt)*dt

模型中关键参数需要根据实车标定：

• lf/lr：前后轴到质心的距离（单位：米）
• m：整车质量（单位：千克）
• Iz：绕Z轴的转动惯量（单位：kg·m²）
• Fyf/Fyr：前后轴侧偏力（与轮胎特性相关）

2.2 轮胎侧偏力建模

轮胎侧偏力的准确建模直接影响控制性能。在小侧偏角范围内，可采用线性模型：

code复制Fyf = -Cf * αf
Fyr = -Cr * αr
αf = δ - (vy + lf*r)/vx
αr = (lr*r - vy)/vx

其中Cf/Cr为前后轮侧偏刚度，需要通过轮胎试验台实测获得。在大侧偏角工况下，则需要采用更复杂的魔术公式（Magic Formula）模型。

注意：实际调试中发现，当侧偏角超过5度时，线性模型误差会急剧增大。建议在高速场景（>60km/h）中使用分段线性化或查表法处理非线性特性。

3. MPC控制器的实现细节

3.1 预测模型离散化处理

将连续时间模型转换为离散时间模型是MPC实现的关键步骤。虽然龙格-库塔法等高阶方法精度更高，但在实时控制中，前向欧拉法因其计算效率更具优势：

code复制# 连续时间状态空间方程
x_dot = Ac*x + Bc*u

# 前向欧拉离散化
Ad = I + Ac*Ts
Bd = Bc*Ts

其中Ts为采样时间，通常取10-50ms。对于车辆控制，当Ts<0.02s时，欧拉法引入的误差可以忽略不计。

3.2 优化问题构建

MPC的核心是求解如下优化问题：

code复制min J = Σ(||x(k+i|k) - x_ref(k+i)||_Q + ||u(k+i)||_R)
s.t. x(k+i+1|k) = f(x(k+i|k), u(k+i))
     u_min ≤ u(k+i) ≤ u_max
     Δu_min ≤ Δu(k+i) ≤ Δu_max

其中Q和R为权重矩阵，需要根据控制目标调整：

• 增大Q(1,1)：加强横向位置跟踪
• 增大Q(4,4)：抑制横摆角速度波动
• 增大R(1,1)：减小方向盘动作幅度

3.3 约束处理技巧

实际工程中，约束条件的处理直接影响控制效果。以下是几个关键约束的实现方法：

1. 执行器速率约束：

code复制-0.3 ≤ δ(k+1) - δ(k) ≤ 0.3 [rad/s]

2. 轮胎摩擦圆约束：

code复制√(a_x² + a_y²) ≤ μ*g

3. 舒适性约束：

code复制|jerk| ≤ 2.5 m/s³

经验分享：在OSQP求解器中，将约束条件按优先级排序可以显著提高求解速度。通常先处理硬约束（如执行器限幅），再处理软约束（如舒适性要求）。

4. 轨迹生成与跟踪实践

4.1 参考轨迹生成方法

高质量的参考轨迹应满足三阶连续（位置、速度、加速度连续）。常用生成方法包括：

1. 双移线轨迹：

code复制y_ref(x) = d/2*(1 + tanh(z1)) - d/2*(1 + tanh(z2))
z1 = 2.4*(x - x1)/d - 1.2
z2 = 2.4*(x - x2)/d - 1.2

2. 五次多项式轨迹：

code复制y_ref(s) = a0 + a1*s + a2*s² + a3*s³ + a4*s⁴ + a5*s⁵

通过边界条件（起终点位置、速度、加速度）求解系数。

4.2 跟踪性能优化技巧

在实际调试中发现以下经验：

1. 预测时域选择：N=15-20（对应1.5-2秒）效果最佳
2. 权重调整策略：
   • 低速时增大位置权重
   • 高速时增大稳定性权重
3. 曲率前馈补偿：

code复制δ_ff = L/R + Kv*a_y

其中Kv为不足转向系数，可通过试验标定。

下表展示了不同速度下的典型控制参数：

5. 实车调试中的问题排查

5.1 常见问题与解决方案

1. 问题：车辆在弯道出口出现振荡

   • 原因：预测时域过短
   • 解决：增加N或调整权重矩阵Q

2. 问题：方向盘频繁抖动

   • 原因：控制周期与CAN总线周期不同步
   • 解决：对齐时间戳或增加控制周期

3. 问题：高速时跟踪误差突然增大

   • 原因：轮胎模型进入非线性区
   • 解决：采用自适应权重或切换模型

5.2 计算性能优化

实时性是车辆控制的核心要求。以下方法可提升计算效率：

1. 热启动技术：复用上一周期的解作为初始猜测
2. 稀疏矩阵优化：利用QP问题的带状结构特性
3. 代码生成：使用CVXGEN等工具生成定制化求解器

在Intel i7-1185G7处理器上，优化前后的性能对比：

6. 进阶技巧与扩展方向

对于希望进一步提升性能的开发者，可以考虑以下方向：

1. 参数自适应：根据车速、载荷等实时更新模型参数
2. 多模MPC：针对不同工况切换模型结构
3. 数据驱动：结合强化学习优化权重参数
4. 容错控制：增加执行器故障检测与重构机制

在冰雪路面试验中，采用参数自适应MPC相比固定参数方案，横向误差减小了37%。实现的关键是在线更新轮胎侧偏刚度：

code复制Cf_hat = Cf_nom * (1 - 0.005*(T - 20))  # 温度补偿
μ_hat = μ_nom * (1 - 0.7*rain_intensity) # 雨量补偿

最后需要强调的是，任何控制算法都需要大量的实车测试验证。建议按照以下阶段逐步推进：

1. 软件在环（SIL）仿真
2. 硬件在环（HIL）测试
3. 封闭场地实车测试
4. 开放道路测试

每个阶段至少需要积累200小时的有效测试数据，才能确保控制算法的可靠性。在实际项目中，我们发现在仿真中表现完美的算法，在实车测试中可能会因为传感器噪声、执行器延迟等现实因素出现性能下降，这时候就需要回到建模阶段重新审视假设条件的合理性。