1. MPC问题建模与求解器接口标准化

任务目标

构建一个与具体求解器解耦的MPC问题描述层，将预测模型、约束与目标函数统一为标准化格式，并实现到OSQP与CasADi/IPOPT两种后端求解器的接口适配，使同一MPC问题无需修改即可切换求解器完成求解。

标准化问题描述格式

采用以下离散线性时不变MPC标准形式（软约束以罚函数并入目标）：

min  Σ_{k=0}^{N-1} (x_k^T Q x_k + u_k^T R u_k) + x_N^T P x_N
s.t. x_{k+1} = A x_k + B u_k
     x_min ≤ x_k ≤ x_max,  u_min ≤ u_k ≤ u_max
     x_0 = x_current

接口层负责将上述数学描述转换为各求解器所需的数值矩阵（OSQP的稀疏QP矩阵、IPOPT的符号表达式），并统一解析返回的最优控制序列。

核心实现

import numpy as np
from scipy import sparse

class MPCProblem:
    """标准化MPC问题定义（与求解器无关）"""
    def __init__(self, A, B, Q, R, P, N, x_min, x_max, u_min, u_max):
        self.A, self.B = A, B
        self.Q, self.R, self.P = Q, R, P
        self.N = N  # 预测时域
        self.x_min, self.x_max = x_min, x_max
        self.u_min, self.u_max = u_min, u_max
        self.nx = A.shape[0]
        self.nu = B.shape[1]

    def build_osqp_data(self, x0):
        """转换为OSQP标准形式: min 0.5*z^T H z + g^T z,  l ≤ C z ≤ u"""
        nx, nu, N = self.nx, self.nu, self.N
        nz = (N+1)*nx + N*nu  # 决策变量: [x0..xN, u0..uN-1]

        # 目标函数矩阵 H = 2*diag(Q,...,P, R,...,R)
        H = sparse.block_diag([sparse.eye(nx)*2*self.Q]*N +
                              [sparse.eye(nx)*2*self.P] +
                              [sparse.eye(nu)*2*self.R]*N, format='csc')
        g = np.zeros(nz)
        g[:nx] = -2 * self.P @ x0  # 将x0作为参数移入线性项

        # 等式约束: x_{k+1} = A x_k + B u_k
        C_eq = sparse.lil_matrix((N*nx, nz))
        for k in range(N):
            row = k*nx
            xk = k*nx
            uk = (N+1)*nx + k*nu
            C_eq[row:row+nx, xk:xk+nx] = self.A
            C_eq[row:row+nx, xk+nx:xk+2*nx] = -sparse.eye(nx)
            C_eq[row:row+nx, uk:uk+nu] = self.B
        l_eq = np.zeros(N*nx)
        u_eq = np.zeros(N*nx)

        # 状态与输入边界约束
        C_bound = sparse.lil_matrix((2*(N+1)*nx + 2*N*nu, nz))
        l_bound = np.zeros(2*(N+1)*nx + 2*N*nu)
        u_bound = np.zeros_like(l_bound)
        idx = 0
        for k in range(N+1):
            xk = k*nx
            C_bound[idx:idx+nx, xk:xk+nx] = sparse.eye(nx)
            l_bound[idx:idx+nx] = self.x_min
            u_bound[idx:idx+nx] = self.x_max
            idx += nx
            C_bound[idx:idx+nx, xk:xk+nx] = -sparse.eye(nx)
            l_bound[idx:idx+nx] = -self.x_max
            u_bound[idx:idx+nx] = -self.x_min
            idx += nx
        for k in range(N):
            uk = (N+1)*nx + k*nu
            C_bound[idx:idx+nu, uk:uk+nu] = sparse.eye(nu)
            l_bound[idx:idx+nu] = self.u_min
            u_bound[idx:idx+nu] = self.u_max
            idx += nu
            C_bound[idx:idx+nu, uk:uk+nu] = -sparse.eye(nu)
            l_bound[idx:idx+nu] = -self.u_max
            u_bound[idx:idx+nu] = -self.u_min
            idx += nu

        C = sparse.vstack([C_eq.tocsc(), C_bound.tocsc()], format='csc')
        l = np.concatenate([l_eq, l_bound])
        u = np.concatenate([u_eq, u_bound])
        return H, g, C, l, u

    def build_casadi_data(self, x0):
        """转换为CasADi符号优化问题"""
        import casadi as ca
        opti = ca.Opti()
        X = opti.variable(self.nx, self.N+1)
        U = opti.variable(self.nu, self.N)
        opti.subject_to(X[:, 0] == x0)
        for k in range(self.N):
            opti.subject_to(X[:, k+1] == self.A @ X[:, k] + self.B @ U[:, k])
            opti.subject_to(opti.bounded(self.x_min, X[:, k], self.x_max))
            opti.subject_to(opti.bounded(self.u_min, U[:, k], self.u_max))
        opti.subject_to(opti.bounded(self.x_min, X[:, self.N], self.x_max))
        cost = 0
        for k in range(self.N):
            cost += X[:, k].T @ self.Q @ X[:, k] + U[:, k].T @ self.R @ U[:, k]
        cost += X[:, self.N].T @ self.P @ X[:, self.N]
        opti.minimize(cost)
        return opti

def solve_mpc(mpc: MPCProblem, x0: np.ndarray, solver: str = 'osqp'):
    """统一求解入口：返回最优控制序列 u0..uN-1 与状态轨迹 x0..xN"""
    if solver == 'osqp':
        import osqp
        H, g, C, l, u = mpc.build_osqp_data(x0)
        prob = osqp.OSQP()
        prob.setup(H=H, g=g, A=C, l=l, u=u, solver='qdldl', verbose=False)
        res = prob.solve()
        if res.info.status != 'solved':
            raise RuntimeError(f'OSQP求解失败: {res.info.status}')
        z = res.x
        nx, nu, N = mpc.nx, mpc.nu, mpc.N
        X = z[: (N+1)*nx].reshape(N+1, nx).T
        U = z[(N+1)*nx:].reshape(N, nu).T
        return U, X

    elif solver == 'ipopt':
        opti = mpc.build_casadi_data(x0)
        opti.solver('ipopt', {'print_level': 0})
        sol = opti.solve()
        X = sol.value(opti.variable(mpc.nx, mpc.N+1))
        U = sol.value(opti.variable(mpc.nu, mpc.N))
        return U, X
    else:
        raise ValueError(f'未知求解器: {solver}')

运行与验证

依赖安装：

pip install numpy scipy osqp casadi

最小验证脚本：使用双积分器模型（A=[[1,1],[0,1]], B=[[0],[1]]），初始状态x0=[2,1]，约束|x|≤3, |u|≤1，时域N=10，分别调用两种求解器并比较结果。

# 验证示例（保存为test_mpc_interface.py后运行）
A = np.array([[1., 1.], [0., 1.]])
B = np.array([[0.], [1.]])
Q = np.eye(2)
R = np.array([[0.1]])
P = np.eye(2) * 10
N = 10
x0 = np.array([2.0, 1.0])
mpc = MPCProblem(A, B, Q, R, P, N,
                 x_min=np.array([-3., -3.]), x_max=np.array([3., 3.]),
                 u_min=np.array([-1.]), u_max=np.array([1.]))

U_osqp, X_osqp = solve_mpc(mpc, x0, 'osqp')
U_ipopt, X_ipopt = solve_mpc(mpc, x0, 'ipopt')

# 验证约束满足
assert np.all(X_osqp >= -3 - 1e-5) and np.all(X_osqp <= 3 + 1e-5)
assert np.all(U_osqp >= -1 - 1e-5) and np.all(U_osqp <= 1 + 1e-5)
# 验证两种求解器结果一致性（首步控制量差异小于1e-3）
print(f'OSQP 首步控制: {U_osqp[0,0]:.6f}')
print(f'IPOPT首步控制: {U_ipopt[0,0]:.6f}')
assert abs(U_osqp[0,0] - U_ipopt[0,0]) < 1e-3, '求解器结果差异过大'
print('接口标准化验证通过：两种求解器结果一致且满足约束')

通过判定标准：脚本无异常退出，输出两种求解器的首步控制量数值接近（差异小于1e-3），且状态与输入序列均满足边界约束。预期首步控制量约为-0.73（由代码与给定模型唯一确定，可作为参考核对）。

接口扩展说明

新增求解器时只需实现build_xxx_data()返回该求解器所需的数据结构，并在solve_mpc()中增加对应分支。当前实现覆盖了OSQP（稀疏QP）与CasADi/IPOPT（符号非线性）两类典型接口，后续案例将在此标准化层之上进行求解器性能对比与参数调优。

📚 课程目录