无人驾驶MPC控制：二自由度模型与轨迹跟踪实现

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

在无人驾驶领域，模型预测控制（MPC）之所以成为轨迹跟踪的首选方案，核心在于它独特的"滚动优化+反馈校正"机制。与传统的PID控制不同，MPC能够在每个控制周期内求解一个有限时域的最优控制问题，这种前瞻性（Look-ahead）特性使其特别适合处理具有时变特性的车辆动力学系统。

1.1 二自由度自行车模型解析

车辆动力学建模是MPC的基础，二自由度自行车模型（Bicycle Model）在精度和复杂度之间取得了良好平衡。这个模型将四轮车辆简化为前后两个轮，主要考虑横向（y轴）和横摆（绕z轴旋转）两个自由度：

code复制m(v̇y + vxφ̇) = Fyf + Fyr
Izφ̈ = lfFyf - lrFyr

其中m为车辆质量，vx/vy分别为纵向/横向速度，φ为横摆角，Fyf/Fyr为前后轮侧向力，lf/lr为前后轴到质心的距离。这个模型的精妙之处在于：

• 通过小角度假设简化了轮胎力的计算
• 保留了车辆转向时的基本动力学特征
• 计算复杂度适合实时控制需求

在实际代码实现中，我们通常采用离散时间状态空间表示：

python复制def discrete_bicycle_model(x, u, dt):
    # x = [y, φ, vy, φ̇]
    # u = [δ, a] 前轮转角和纵向加速度
    β = np.arctan2(lr * np.tan(u[0]), lf + lr)
    Fyf = Cf * (u[0] - (x[3] + lf*x[4]/x[2]))
    Fyr = Cr * (- (x[3] - lr*x[4]/x[2]))
    
    dxdt = [
        x[2]*np.cos(x[1]) - x[3]*np.sin(x[1]),  # ẏ
        x[4],                                  # φ̇
        (Fyf + Fyr)/m - x[4]*x[2],             # v̇y
        (lf*Fyf - lr*Fyr)/Iz,                 # φ̈
        u[1]