自动驾驶MPC控制器前传：手把手推导车辆模型的离散状态空间方程（Python/NumPy版）

秦哲祺

213人浏览 · 2026-03-26 10:08:15

秦哲祺 · 2026-03-26 10:08:15 发布

自动驾驶MPC控制器前传：从运动学模型到离散状态空间的完整推导与实践

在自动驾驶系统的开发中，模型预测控制（MPC）因其优秀的处理约束能力和前瞻性而广受青睐。然而，许多开发者在尝试实现自己的MPC控制器时，往往会在第一步——构建合适的车辆预测模型上遇到困难。本文将深入探讨如何从基础的单车运动学模型出发，通过严谨的数学推导和Python实现，构建适用于MPC控制器的离散状态空间模型。

1. 为什么MPC需要离散状态空间模型

模型预测控制的核心思想是通过优化未来一段时间内的控制输入序列，使得系统输出尽可能接近期望轨迹。这一过程需要：

预测模型：能够根据当前状态和控制输入预测未来状态
优化问题：在考虑各种约束条件下最小化目标函数
滚动时域：每次只执行第一个控制输入，然后重新优化

对于自动驾驶车辆这样的非线性系统，直接使用原始运动学方程进行优化计算量巨大。离散状态空间模型提供了几个关键优势：

线性结构：将非线性系统在工作点附近线性化，大大简化优化计算
离散时间：与数字控制系统的采样特性完美匹配
矩阵形式：便于利用高效的数值优化算法求解

提示：虽然线性化会引入一定误差，但在短时间内（MPC的预测时域通常只有几秒）这种近似通常是可接受的。

2. 车辆运动学模型基础

我们采用以后轴中心为参考点的单车模型，这是自动驾驶控制中最常用的简化模型之一。该模型假设：

车辆在二维平面内运动
不考虑轮胎滑移等动力学效应
前轮转向角是直接可控量

模型的连续时间表达式为：

def continuous_kinematic_model(state, control):
    x, y, psi = state  # 位置和航向角
    v, delta = control  # 速度和前轮转角
    L = 2.9  # 车辆轴距
    
    x_dot = v * math.cos(psi)
    y_dot = v * math.sin(psi)
    psi_dot = v * math.tan(delta) / L
    
    return np.array([x_dot, y_dot, psi_dot])

这个模型虽然简洁，但包含了车辆运动的基本特性，非常适合作为MPC控制的基础预测模型。

3. 模型线性化：从非线性到线性状态空间

为了将非线性模型转换为线性形式，我们需要在工作点（参考轨迹上的点）附近进行泰勒展开并保留一阶项。这一过程涉及几个关键步骤：

3.1 参考点和误差状态定义

设参考轨迹上的状态和控制输入为χᵣ和uᵣ，定义误差状态： χ̃ = χ - χᵣ ũ = u - uᵣ

3.2 雅可比矩阵计算

线性化的核心是计算系统动态方程对状态和控制的偏导数，即雅可比矩阵。对于我们的运动学模型：

状态矩阵A：

A = np.zeros((3,3))
A[0,2] = -v_r * math.sin(psi_r)
A[1,2] = v_r * math.cos(psi_r)

控制矩阵B：

B = np.zeros((3,2))
B[0,0] = math.cos(psi_r)
B[1,0] = math.sin(psi_r)
B[2,0] = math.tan(delta_r)/L
B[2,1] = v_r/(L * math.cos(delta_r)**2)

这样得到的线性化误差状态方程为： χ̃̇ = Aχ̃ + Bũ

4. 离散化：适配数字控制系统

连续时间状态空间模型需要转换为离散时间形式才能用于数字控制器。我们采用前向欧拉方法，这是最简单直观的离散化方法之一。

4.1 前向欧拉离散化

对于采样周期T，离散化公式为： χ̃(k+1) ≈ χ̃(k) + T·χ̃̇(k) = (I + TA)χ̃(k) + TBũ(k)

对应的Python实现：

def discrete_state_space(v_r, psi_r, delta_r, L, dt):
    """计算离散状态空间矩阵"""
    A = np.eye(3)
    A[0,2] = -v_r * math.sin(psi_r) * dt
    A[1,2] = v_r * math.cos(psi_r) * dt
    
    B = np.zeros((3,2))
    B[0,0] = math.cos(psi_r) * dt
    B[1,0] = math.sin(psi_r) * dt
    B[2,0] = math.tan(delta_r) * dt / L
    B[2,1] = v_r * dt / (L * math.cos(delta_r)**2)
    
    return A, B

4.2 离散化方法的比较

方法	精度	计算复杂度	适用场景
前向欧拉	低	低	简单系统，快速实现
后向欧拉	中	中	刚性系统
零阶保持	高	高	高性能控制器
双线性变换	高	高	需要高精度场合

对于大多数自动驾驶应用，前向欧拉方法在采样时间足够短（如50-100ms）时已经能够提供足够的精度。

5. 完整Python实现与验证

我们将上述步骤整合到一个完整的Python类中，方便在实际MPC控制器中使用：

class LinearDiscreteVehicleModel:
    def __init__(self, L=2.9, dt=0.1):
        self.L = L  # 车辆轴距
        self.dt = dt  # 采样时间
        
    def update_reference(self, v_r, psi_r, delta_r):
        """更新参考点参数并重新计算状态空间矩阵"""
        self.v_r = v_r
        self.psi_r = psi_r
        self.delta_r = delta_r
        
        # 计算离散状态空间矩阵
        self.Ad = np.eye(3)
        self.Ad[0,2] = -v_r * math.sin(psi_r) * self.dt
        self.Ad[1,2] = v_r * math.cos(psi_r) * self.dt
        
        self.Bd = np.zeros((3,2))
        self.Bd[0,0] = math.cos(psi_r) * self.dt
        self.Bd[1,0] = math.sin(psi_r) * self.dt
        self.Bd[2,0] = math.tan(delta_r) * self.dt / self.L
        self.Bd[2,1] = v_r * self.dt / (self.L * math.cos(delta_r)**2)
        
    def predict(self, state_error, control_error):
        """预测下一时刻的状态误差"""
        return self.Ad @ state_error + self.Bd @ control_error

验证示例：

model = LinearDiscreteVehicleModel(dt=0.1)
model.update_reference(v_r=5.0, psi_r=0.3, delta_r=0.1)

# 初始误差
chi_tilde = np.array([0.5, -0.2, 0.1])
u_tilde = np.array([0.1, 0.05])

# 预测下一步
chi_tilde_next = model.predict(chi_tilde, u_tilde)