1
2
3
4
5
6

在时刻 k：
1. 获取当前状态 x(k)
2. 利用模型预测未来 N 步的状态
3. 求解优化问题，得到控制序列 u(k), u(k+1), ..., u(k+N-1)
4. 只执行第一个控制 u(k)
5. 等待下一个采样时刻，重复以上步骤

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92

import numpy as np
from scipy.optimize import minimize

class MPC:
    def __init__(self, A, B, N, Q, R, umin, umax):
        """
        MPC控制器
        
        参数:
            A, B: 系统矩阵
            N: 预测时域
            Q: 状态权重
            R: 控制权重
            umin, umax: 控制约束
        """
        self.A = A
        self.B = B
        self.N = N
        self.Q = Q
        self.R = R
        self.umin = umin
        self.umax = umax
        
        self.nx = A.shape[0]  # 状态维度
        self.nu = B.shape[1]  # 输入维度
    
    def predict(self, x0, U):
        """预测未来状态"""
        X = np.zeros((self.N + 1, self.nx))
        X[0] = x0
        for i in range(self.N):
            X[i + 1] = self.A @ X[i] + self.B @ U[i]
        return X
    
    def cost(self, U, x0, xref):
        """目标函数"""
        U = U.reshape(self.N, self.nu)
        X = self.predict(x0, U)
        
        # 状态代价
        state_cost = 0
        for i in range(1, self.N + 1):
            state_cost += (X[i] - xref).T @ self.Q @ (X[i] - xref)
        
        # 控制代价
        control_cost = 0
        for i in range(self.N):
            control_cost += U[i].T @ self.R @ U[i]
        
        return state_cost + control_cost
    
    def solve(self, x0, xref):
        """求解MPC问题"""
        # 初始猜测
        U0 = np.zeros(self.N * self.nu)
        
        # 控制约束
        bounds = [(self.umin, self.umax)] * self.N * self.nu
        
        # 求解优化问题
        result = minimize(
            self.cost, U0, args=(x0, xref),
            bounds=bounds, method='SLSQP'
        )
        
        # 返回第一个控制输入
        U_opt = result.x.reshape(self.N, self.nu)
        return U_opt[0]


# 示例：双积分器系统
A = np.array([[1, 0.1], [0, 1]])
B = np.array([[0], [0.1]])

# MPC参数
N = 10
Q = np.eye(2)
R = np.array([[0.1]])
umin, umax = -1, 1

# 创建MPC控制器
mpc = MPC(A, B, N, Q, R, umin, umax)

# 模拟
x = np.array([2, 0])  # 初始状态
xref = np.array([0, 0])  # 目标状态

print("时间 | 位置 | 速度 | 控制输入")
for t in range(30):
    u = mpc.solve(x, xref)
    print(f"{t:4d} | {x[0]:.3f} | {x[1]:.3f} | {u[0]:.3f}")
    x = A @ x + B @ u