MPC 模型预测控制：从状态空间建模到 QP 求解的 3 步实战推导

MPC 模型预测控制从状态空间建模到 QP 求解的 3 步实战推导1. 状态空间建模与预测方程构建在 MPC 的工程实践中状态空间模型是预测未来系统行为的基石。我们以一个典型的离散时间线性系统为例其状态空间方程可表示为# 离散状态空间方程示例 A np.array([[1, 0.1], [0, 0.9]]) # 状态转移矩阵 B np.array([[0.05], [0.8]]) # 控制输入矩阵 x_k np.array([[2.0], [1.5]]) # 当前状态 u_k np.array([[0.5]]) # 当前控制输入 # 下一时刻状态预测 x_k1 A x_k B u_k预测时域内的状态序列可通过迭代计算得到。对于预测步长 $P5$控制步长 $M3$预测方程的矩阵形式为$$ \begin{bmatrix} x_{k1|k} \ x_{k2|k} \ \vdots \ x_{kP|k} \end{bmatrix} \begin{bmatrix} A \ A^2 \ \vdots \ A^P \end{bmatrix}x_k \begin{bmatrix} B 0 \cdots 0 \ AB B \ddots \vdots \ \vdots \vdots \ddots 0 \ A^{P-1}B A^{P-2}B \cdots A^{P-M}B \end{bmatrix} \begin{bmatrix} u_k \ u_{k1} \ \vdots \ u_{kM-1} \end{bmatrix} $$关键操作步骤确定系统矩阵 $A$ 和 $B$ 的维度构建预测矩阵 $\mathcal{A}$ 和 $\mathcal{B}$处理控制时域 $M$ 小于预测时域 $P$ 的情况考虑输出方程 $y_k Cx_k$ 的扩展提示对于非线性系统可通过泰勒展开在工作点附近进行线性化处理得到近似的状态空间模型。2. 代价函数设计与 QP 标准形式转换MPC 的核心优化问题通常采用二次型代价函数$$ J \sum_{i1}^{P} |y_{ki} - r_{ki}|Q^2 \sum{j0}^{M-1} |u_{kj}|R^2 \sum{j0}^{M-1} |\Delta u_{kj}|_S^2 $$将其转换为标准 QP 问题形式$$ \min_{\mathbf{u}} \frac{1}{2}\mathbf{u}^T H \mathbf{u} f^T \mathbf{u} $$其中关键矩阵的计算过程如下表所示矩阵元素计算公式维度说明$H$$2(\mathcal{B}^T \bar{Q} \mathcal{B} \bar{R} \mathcal{D}^T \bar{S} \mathcal{D})$$M \times n_u$$f$$2\mathcal{B}^T \bar{Q} (\mathcal{A}x_k - \mathbf{r})$$M \times 1$$\bar{Q}$$\text{blkdiag}(C^T Q C, \cdots, C^T Q C)$$P \times P$ 块对角矩阵$\mathcal{D}$控制增量变换矩阵$M \times M$# Python代码示例构建QP问题 def build_qp_matrices(A, B, C, Q, R, P, M, x_k, r_sequence): # 构建预测矩阵 Phi np.vstack([np.linalg.matrix_power(A, i1) for i in range(P)]) Gamma np.zeros((P*C.shape[0], M*B.shape[1])) for i in range(P): for j in range(min(i1, M)): Gamma[i*C.shape[0]:(i1)*C.shape[0], j*B.shape[1]:(j1)*B.shape[1]] C np.linalg.matrix_power(A, i-j) B # 构建权重矩阵 Q_bar np.kron(np.eye(P), Q) R_bar np.kron(np.eye(M), R) # 计算H和f H 2 * (Gamma.T Q_bar Gamma R_bar) f 2 * Gamma.T Q_bar (Phi x_k - r_sequence) return H, f3. 约束处理与 QP 求解实战实际工程中的约束条件主要包括输入约束$u_{min} \leq u_k \leq u_{max}$输入变化率约束$\Delta u_{min} \leq u_k - u_{k-1} \leq \Delta u_{max}$状态约束$x_{min} \leq x_k \leq x_{max}$这些约束可统一表示为$$ A_{ineq} \mathbf{u} \leq b_{ineq} $$使用 OSQP 求解器的典型流程# OSQP求解示例 import osqp from scipy import sparse # 构建QP问题数据 H_sparse sparse.csc_matrix(H) A_sparse sparse.csc_matrix(A_ineq) # 创建OSQP对象 prob osqp.OSQP() prob.setup(H_sparse, f, A_sparse, l, u, verboseFalse) # 求解 results prob.solve() u_opt results.x[:M] # 提取最优控制序列约束处理技巧将状态约束转换为输入约束的等效形式处理不等式约束时的松弛变量引入热启动策略加速迭代求解4. 完整实现案例车辆轨迹跟踪控制考虑车辆横向运动控制问题采用自行车模型# 车辆模型参数 dt 0.1 # 时间步长 L 2.5 # 轴距 v 5.0 # 纵向速度 # 状态空间模型 A np.array([[1, v*dt, 0], [0, 1, v*dt/L], [0, 0, 1]]) B np.array([[0], [0], [dt/L]]) # MPC控制器实现 class MPCController: def __init__(self, P10, M5): self.P P # 预测时域 self.M M # 控制时域 self.Q np.diag([10, 1, 0.1]) # 状态权重 self.R np.eye(1) * 0.01 # 输入权重 def solve(self, x0, ref_path): # 构建QP问题 H, f build_qp_matrices(A, B, np.eye(3), self.Q, self.R, self.P, self.M, x0, ref_path) # 设置输入约束 u_min np.array([-np.pi/6]) # 最大转向角30度 u_max np.array([np.pi/6]) # 求解QP prob osqp.OSQP() prob.setup(sparse.csc_matrix(H), f, None, None, bounds(np.tile(u_min, self.M), np.tile(u_max, self.M))) results prob.solve() return results.x[0] if results.info.status solved else 0.0实际调试经验预测时域 $P$ 通常选择系统响应时间的1.5-2倍控制时域 $M$ 一般为 $P$ 的1/3到1/2权重矩阵 $Q$ 和 $R$ 需要平衡响应速度与控制量大小采样时间 $dt$ 应小于系统最小时间常数的1/10在实现过程中我发现将预测方程转换为QP标准形式时矩阵维度的对齐至关重要。特别是在处理多输入多输出系统时需要仔细验证每个子块的维度匹配情况。