├── .gitignore ├── README.md ├── docs ├── 1-s2.0-S0005109804002870-main.pdf ├── Invariant_approximations_of_robustly_positively_in.pdf ├── Linear_Systems_with_Output_Constraints_The_Theory_and_Application_of_Maximal_Output_Admissible_Sets.pdf ├── fourier-motzkin-elimination[1] └── 笔记.pdf ├── pyproject.toml ├── results ├── lqr_and_linear_mpc │ ├── fig_1.gif │ ├── fig_2.png │ └── fig_3.png ├── poly_test │ ├── fig_1.png │ ├── fig_2.png │ ├── fig_3.png │ ├── fig_4.png │ ├── fig_5.png │ ├── fig_6.png │ └── fig_7.png ├── polyhedron_and_ellipsoid_terminal_set │ ├── fig_1.gif │ ├── fig_2.gif │ ├── fig_3.png │ └── fig_4.png └── tube_based_mpc │ ├── fig_1.gif │ ├── fig_2.png │ ├── fig_3.png │ └── fig_4.gif ├── src └── tmpc │ ├── __init__.py │ ├── mpc │ ├── __init__.py │ ├── base.py │ ├── exception.py │ ├── mpc.py │ └── tube_based_mpc.py │ └── set │ ├── __init__.py │ ├── base.py │ ├── ellipsoid.py │ ├── exception.py │ └── poly.py └── tests ├── lqr_and_linear_mpc.py ├── poly_test.py ├── polyhedron_and_ellipsoid_terminal_set.py └── tube_based_mpc.py /.gitignore: -------------------------------------------------------------------------------- 1 | **/__pycache__/ 2 | **/build/ 3 | **/*.egg-info/ 4 | .vscode/ 5 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |

2 | English | 中文 3 |

4 | 5 | --- 6 | 7 | 8 | # Rigid-tube MPC 9 | Reproduction of the Simulation Section from the Paper 10 | [D. Q. Mayne, M. M. Seron, and S. V. Raković, "Robust Model Predictive Control of Constrained Linear Systems with Bounded Disturbances," Automatica, vol. 41, no. 2, pp. 219–224, 2005.](https://www.sciencedirect.com/science/article/pii/S0005109804002870) 11 | Additionally, comparative cases incorporating LQR (Linear Quadratic Regulator) and Linear MPC (Model Predictive Control) have been included. 12 | 13 | ## Installation 14 | ### 1. Create and Activate Virtual Environment (Optional but Recommended) 15 | ```bash 16 | # For Linux/macOS 17 | python -m venv .venv 18 | source .venv/bin/activate 19 | 20 | # For Windows 21 | python -m venv .venv 22 | .\.venv\Scripts\activate 23 | ``` 24 | 25 | ### 2. Install Package with Dependencies 26 | #### Method 1: Install directly from GitHub (Recommended) 27 | ```bash 28 | pip install git+https://github.com/rui-huang-opt/tmpc.git 29 | ``` 30 | 31 | #### Method 2: Clone repository and install 32 | ```bash 33 | git clone https://github.com/rui-huang-opt/tmpc.git 34 | cd tmpc 35 | 36 | # Install the package and its dependencies (automatically resolved from pyproject.toml). 37 | pip install . 38 | ``` 39 | 40 | --- 41 | 42 | 43 | # Rigid-tube MPC 44 | 关于论文[D. Q. Mayne, M. M. Seron, and S. V. Raković, “Robust model predictive control of constrained linear systems with bounded disturbances,” Automatica, vol. 41, no. 2, pp. 219–224, 2005.](https://www.sciencedirect.com/science/article/pii/S0005109804002870)中仿真部分的复现。此外，还加入了LQR和线性MPC的对比案例。 45 | 46 | ## 安装 47 | ### 1. 创建虚拟环境 (非必须但是推荐这样做) 48 | ```bash 49 | # Linux/macOS 系统 50 | python -m venv .venv 51 | source .venv/bin/activate 52 | 53 | # Windows 系统 54 | python -m venv .venv 55 | .\.venv\Scripts\activate 56 | ``` 57 | 58 | ### 2. 安装包（以及依赖） 59 | #### 方法1：直接从 GitHub 安装（推荐） 60 | ```bash 61 | pip install git+https://github.com/rui-huang-opt/tmpc.git 62 | ``` 63 | 64 | #### 方法2：克隆仓库后安装 65 | ```bash 66 | git clone https://github.com/rui-huang-opt/tmpc.git 67 | cd tmpc 68 | 69 | # 安装包及其依赖（自动从 pyproject.toml 读取） 70 | pip install . 71 | ``` 72 | 73 | ## 多面体类测试结果 74 | ### 多面体平移 75 | ![img](results/poly_test/fig_1.png) 76 | 77 | ### 多面体间闵可夫斯基和 78 | ![img](results/poly_test/fig_2.png) 79 | 80 | ### 多面体间庞特里亚金差 81 | 下图表示庞特里亚金差不是将集合内所有向量取反再闵可夫斯基和 82 | 83 | ![img](results/poly_test/fig_3.png) 84 | 85 | ### 线性坐标变换（对集合进行矩阵乘法） 86 | 实际可以进行升维和降维，这里未展示 87 | 88 | ![img](results/poly_test/fig_4.png) 89 | 90 | ### 向量空间 91 | ![img](results/poly_test/fig_5.png) 92 | 93 | ### 单位立方体 94 | ![img](results/poly_test/fig_6.png) 95 | 96 | ### 一个多面体内的最大椭球（球心确定） 97 | ![img](results/poly_test/fig_7.png) 98 | 99 | ## LQR 与 MPC 对比结果 100 | ### 状态轨迹对比 101 | ![img](results/lqr_and_linear_mpc/fig_1.gif) 102 | 103 | ### 输入序列对比 104 | ![img](results/lqr_and_linear_mpc/fig_2.png) 105 | 106 | ### MPC初始状态可行域 107 | 初始状态属于这个集合问题才可解 108 | 109 | ![img](results/lqr_and_linear_mpc/fig_3.png) 110 | 111 | ## 多面体终端集椭球终端集 112 | ### 状态轨迹对比 113 | 可以看出离稳定点越远，区别越大，反之越小，但都可以稳定（多面体终端集初始可行域更大） 114 | 115 | ![img](results/polyhedron_and_ellipsoid_terminal_set/fig_1.gif) 116 | ![img](results/polyhedron_and_ellipsoid_terminal_set/fig_2.gif) 117 | 118 | ### 输入序列对比 119 | ![img](results/polyhedron_and_ellipsoid_terminal_set/fig_3.png) 120 | ![img](results/polyhedron_and_ellipsoid_terminal_set/fig_4.png) 121 | 122 | ## Tube based MPC结果 123 | ### 状态轨迹 124 | 实际状态始终在以名义状态为中心的管道内 125 | 126 | ![img](results/tube_based_mpc/fig_1.gif) 127 | 128 | ### 输入序列 129 | 分为两部分，不考虑噪声的名义系统输入和用于抑制噪声的输入 130 | 131 | ![img](results/tube_based_mpc/fig_2.png) 132 | 133 | ### Tube based MPC初始状态可行域 134 | 蓝色为实际可行域，红色表示控制器内预测状态序列的第一步的状态的可行域 135 | 136 | ![img](results/tube_based_mpc/fig_3.png) 137 | 138 | ### 正鲁棒不变集测试效果 139 | 下图说明一个状态方程为 $x_{k+1}=Ax_{k}+w$的系统，其中w为有界噪声，则当它进入鲁棒不变集后就不会再出去 140 | 141 | ![img](results/tube_based_mpc/fig_4.gif) -------------------------------------------------------------------------------- /docs/1-s2.0-S0005109804002870-main.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/docs/1-s2.0-S0005109804002870-main.pdf -------------------------------------------------------------------------------- /docs/Invariant_approximations_of_robustly_positively_in.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/docs/Invariant_approximations_of_robustly_positively_in.pdf -------------------------------------------------------------------------------- /docs/Linear_Systems_with_Output_Constraints_The_Theory_and_Application_of_Maximal_Output_Admissible_Sets.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/docs/Linear_Systems_with_Output_Constraints_The_Theory_and_Application_of_Maximal_Output_Admissible_Sets.pdf -------------------------------------------------------------------------------- /docs/笔记.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/docs/笔记.pdf -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["setuptools", "wheel"] 3 | build-backend = "setuptools.build_meta" 4 | 5 | [project] 6 | requires-python = ">=3.9" 7 | name = "tmpc" 8 | version = "0.1.0" 9 | description = "Tube-based Model Predictive Control." 10 | authors = [ 11 | {name = "Rui Huang", email = "ruihuang0421@gmail.com"}, 12 | ] 13 | 14 | dependencies = [ 15 | "clarabel==0.6.0", 16 | "contourpy==1.2.0", 17 | "cvxopt==1.3.2", 18 | "cvxpy==1.4.2", 19 | "cvxpy-base==1.4.2", 20 | "cycler==0.12.1", 21 | "ecos==2.0.13", 22 | "fonttools==4.49.0", 23 | "importlib_resources==6.1.2", 24 | "kiwisolver==1.4.5", 25 | "matplotlib==3.7.4", 26 | "numpy==1.26.4", 27 | "osqp==0.6.5", 28 | "packaging==23.2", 29 | "pillow==10.2.0", 30 | "piqp==0.2.4", 31 | "pybind11==2.11.1", 32 | "pyparsing==3.1.1", 33 | "python-dateutil==2.8.2", 34 | "qdldl==0.1.7.post0", 35 | "scipy==1.11.4", 36 | "scs==3.2.4.post1", 37 | "six==1.16.0", 38 | "zipp==3.17.0", 39 | ] 40 | 41 | [tool.setuptools.packages.find] 42 | where = ["src"] 43 | -------------------------------------------------------------------------------- /results/lqr_and_linear_mpc/fig_1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/results/lqr_and_linear_mpc/fig_1.gif -------------------------------------------------------------------------------- /results/lqr_and_linear_mpc/fig_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/results/lqr_and_linear_mpc/fig_2.png -------------------------------------------------------------------------------- /results/lqr_and_linear_mpc/fig_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/results/lqr_and_linear_mpc/fig_3.png -------------------------------------------------------------------------------- /results/poly_test/fig_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/results/poly_test/fig_1.png -------------------------------------------------------------------------------- /results/poly_test/fig_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/results/poly_test/fig_2.png -------------------------------------------------------------------------------- /results/poly_test/fig_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/results/poly_test/fig_3.png -------------------------------------------------------------------------------- /results/poly_test/fig_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/results/poly_test/fig_4.png -------------------------------------------------------------------------------- /results/poly_test/fig_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/results/poly_test/fig_5.png -------------------------------------------------------------------------------- /results/poly_test/fig_6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/results/poly_test/fig_6.png -------------------------------------------------------------------------------- /results/poly_test/fig_7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/results/poly_test/fig_7.png -------------------------------------------------------------------------------- /results/polyhedron_and_ellipsoid_terminal_set/fig_1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/results/polyhedron_and_ellipsoid_terminal_set/fig_1.gif -------------------------------------------------------------------------------- /results/polyhedron_and_ellipsoid_terminal_set/fig_2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/results/polyhedron_and_ellipsoid_terminal_set/fig_2.gif -------------------------------------------------------------------------------- /results/polyhedron_and_ellipsoid_terminal_set/fig_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/results/polyhedron_and_ellipsoid_terminal_set/fig_3.png -------------------------------------------------------------------------------- /results/polyhedron_and_ellipsoid_terminal_set/fig_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/results/polyhedron_and_ellipsoid_terminal_set/fig_4.png -------------------------------------------------------------------------------- /results/tube_based_mpc/fig_1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/results/tube_based_mpc/fig_1.gif -------------------------------------------------------------------------------- /results/tube_based_mpc/fig_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/results/tube_based_mpc/fig_2.png -------------------------------------------------------------------------------- /results/tube_based_mpc/fig_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/results/tube_based_mpc/fig_3.png -------------------------------------------------------------------------------- /results/tube_based_mpc/fig_4.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rui-huang-opt/tmpc/039757b1c3e5492a80b5d9180e63c6d1e4e84dca/results/tube_based_mpc/fig_4.gif -------------------------------------------------------------------------------- /src/tmpc/__init__.py: -------------------------------------------------------------------------------- 1 | __all__ = ["mpc", "set"] 2 | 3 | from . import * 4 | -------------------------------------------------------------------------------- /src/tmpc/mpc/__init__.py: -------------------------------------------------------------------------------- 1 | from .base import LQR 2 | from .mpc import MPC 3 | from .tube_based_mpc import TubeBasedMPC 4 | -------------------------------------------------------------------------------- /src/tmpc/mpc/base.py: -------------------------------------------------------------------------------- 1 | import abc 2 | import copy 3 | import cvxpy as cp 4 | import numpy as np 5 | import numpy.linalg as npl 6 | import scipy.linalg as spl 7 | from typing import Union, Tuple 8 | from functools import cache 9 | from numpy import number 10 | from numpy.typing import NDArray 11 | from .exception import * 12 | from ..set import Polyhedron, Ellipsoid, unit_cube 13 | 14 | 15 | class LQR: 16 | def __init__(self, a: NDArray[number], b: NDArray[number], q: NDArray[number], r: NDArray[number]): 17 | if not (a.ndim == b.ndim == q.ndim == r.ndim == 2): 18 | raise MPCTypeException("A, B, Q, R", "2D array") 19 | if not (a.shape[0] == a.shape[1] == b.shape[0] == q.shape[0] == q.shape[1]): 20 | raise MPCDimensionException("A, B, Q") 21 | if not (b.shape[1] == r.shape[0] == r.shape[1]): 22 | raise MPCDimensionException("B, R") 23 | 24 | self.__state_dim = a.shape[1] 25 | self.__input_dim = b.shape[1] 26 | 27 | self.__a = a 28 | self.__b = b 29 | self.__q = q 30 | self.__r = r 31 | 32 | self.__p, self.__k = self.cal_lqr() 33 | 34 | @property 35 | def state_dim(self) -> int: 36 | return self.__state_dim 37 | 38 | @property 39 | def input_dim(self) -> int: 40 | return self.__input_dim 41 | 42 | @property 43 | def a(self) -> NDArray[number]: 44 | return self.__a 45 | 46 | @property 47 | def b(self) -> NDArray[number]: 48 | return self.__b 49 | 50 | @property 51 | def q(self) -> NDArray[number]: 52 | return self.__q 53 | 54 | @property 55 | def r(self) -> NDArray[number]: 56 | return self.__r 57 | 58 | @property 59 | def p(self) -> NDArray[number]: 60 | return self.__p 61 | 62 | @property 63 | def k(self) -> NDArray[number]: 64 | return self.__k 65 | 66 | def cal_lqr(self) -> Tuple[NDArray[number], NDArray[number]]: 67 | p = spl.solve_discrete_are(self.__a, self.__b, self.__q, self.__r) 68 | k = npl.inv(self.__r + self.__b.T @ p @ self.__b) @ self.__b.T @ p @ self.__a 69 | 70 | return p, k 71 | 72 | def __call__(self, real_time_state: NDArray[number]) -> NDArray[number]: 73 | if real_time_state.ndim != 1: 74 | raise MPCTypeException("real time state", "1D array") 75 | if real_time_state.shape[0] != self.__state_dim: 76 | raise MPCDimensionException("real time state and state in controller") 77 | 78 | return -self.__k @ real_time_state 79 | 80 | 81 | class MPCBase(LQR, metaclass=abc.ABCMeta): 82 | def __init__( 83 | self, 84 | a: NDArray[number], 85 | b: NDArray[number], 86 | q: NDArray[number], 87 | r: NDArray[number], 88 | pred_horizon: int, 89 | terminal_set_type: str, 90 | solver: str, 91 | ): 92 | super().__init__(a, b, q, r) 93 | 94 | if pred_horizon <= 0: 95 | raise MPCTypeException("prediction horizon", "positive integer") 96 | if terminal_set_type not in ["zero", "ellipsoid", "polyhedron"]: 97 | raise MPCTerminalSetTypeException() 98 | 99 | self.__pred_horizon = pred_horizon 100 | 101 | self.__real_time_state = cp.Parameter(self.state_dim) 102 | self.__state_series = cp.Variable(self.state_dim * (pred_horizon + 1)) 103 | self.__input_series = cp.Variable(self.input_dim * pred_horizon) 104 | 105 | self.__terminal_set_type = terminal_set_type 106 | 107 | self.__solver = solver 108 | 109 | @property 110 | def pred_horizon(self) -> int: 111 | return self.__pred_horizon 112 | 113 | @pred_horizon.setter 114 | def pred_horizon(self, value: int) -> None: 115 | if value <= 0: 116 | raise MPCTypeException("prediction horizon", "positive integer") 117 | 118 | self.__pred_horizon = value 119 | 120 | @property 121 | @abc.abstractmethod 122 | def state_set(self) -> Polyhedron: ... 123 | 124 | @property 125 | @abc.abstractmethod 126 | def input_set(self) -> Polyhedron: ... 127 | 128 | @property 129 | def state_prediction_series(self) -> NDArray[number]: 130 | return self.__state_series.value.reshape(self.__pred_horizon + 1, self.state_dim).T 131 | 132 | @property 133 | def input_prediction_series(self) -> NDArray[number]: 134 | return self.__input_series.value.reshape(self.__pred_horizon, self.input_dim).T 135 | 136 | @property 137 | def input_ini(self) -> cp.Variable: 138 | return self.__input_series[0 : self.input_dim] 139 | 140 | @property 141 | def state_ini(self) -> cp.Variable: 142 | return self.__state_series[0 : self.state_dim] 143 | 144 | @property 145 | def real_time_state(self) -> cp.Parameter: 146 | return self.__real_time_state 147 | 148 | @real_time_state.setter 149 | def real_time_state(self, value: NDArray[number]) -> None: 150 | if value.ndim != 1: 151 | raise MPCTypeException("real time state", "1D array") 152 | if value.size != self.state_dim: 153 | raise MPCDimensionException("real time state and state in controller") 154 | 155 | self.__real_time_state.value = value 156 | 157 | @property 158 | def terminal_set_type(self) -> str: 159 | return self.__terminal_set_type 160 | 161 | @terminal_set_type.setter 162 | def terminal_set_type(self, value: str) -> None: 163 | if value not in ["zero", "ellipsoid", "polyhedron"]: 164 | raise MPCTerminalSetTypeException 165 | 166 | self.__terminal_set_type = value 167 | 168 | @property 169 | def solver(self) -> str: 170 | return self.__solver 171 | 172 | @solver.setter 173 | def solver(self, value) -> None: 174 | self.__solver = value 175 | 176 | @property 177 | @abc.abstractmethod 178 | def initial_constraint(self) -> cp.Constraint: ... 179 | 180 | @abc.abstractmethod 181 | def __call__(self, real_time_state: NDArray[number]) -> NDArray[number]: ... 182 | 183 | @property 184 | @cache 185 | def terminal_set(self) -> Union[Polyhedron, Ellipsoid]: 186 | # 在终端约束 Xf 内的一点 x 满足： 187 | # 1. 当采用控制律 u = Kx 时，状态约束和输入约束均满足 -- 这一条件描述的集合为 X 与 U @ K 的交集，集合与矩阵的乘法解释请参考文件poly 188 | # 2. 下一时刻的状态 x+ = A_k @ x 仍属于 Xf -- 这一条件描述的集合 set 被包含于 set @ A_k 189 | # 若设置终端约束集合为原点，则生成一个边长为0的单位立方体，否则计算最大的满足上述条件的集合 190 | if self.__terminal_set_type == "zero": 191 | terminal_set = unit_cube(self.state_dim, 0) 192 | elif self.__terminal_set_type == "ellipsoid": 193 | state_set_in_terminal = self.state_set & (self.input_set @ self.k) 194 | terminal_set = state_set_in_terminal.get_max_ellipsoid(self.p / 2) 195 | else: 196 | set_k = self.state_set & (self.input_set @ self.k) 197 | terminal_set = copy.deepcopy(set_k) 198 | a_k = self.a - self.b @ self.k 199 | 200 | while True: 201 | set_k = set_k @ a_k 202 | terminal_set_next = terminal_set & set_k 203 | 204 | if terminal_set.subset_eq(terminal_set_next): 205 | break 206 | 207 | terminal_set = terminal_set_next 208 | 209 | return terminal_set 210 | 211 | @property 212 | @cache 213 | def problem(self) -> cp.Problem: 214 | cost = 0 215 | state_k = self.__state_series[0 : self.state_dim] 216 | 217 | # 对于初始状态的约束 218 | constraints = [self.initial_constraint] 219 | 220 | for k in range(self.__pred_horizon): 221 | input_k = self.__input_series[k * self.input_dim : (k + 1) * self.input_dim] 222 | 223 | # l(x, u) = x.T @ Q @ x + u.T @ R @ u 224 | cost = cost + (state_k @ self.q @ state_k + input_k @ self.r @ input_k) / 2 225 | 226 | # x^+ = A @ x + B @ u 227 | state_k_next = self.__state_series[(k + 1) * self.state_dim : (k + 2) * self.state_dim] 228 | constraints.append(state_k_next == self.a @ state_k + self.b @ input_k) 229 | 230 | # x in X, u in U 231 | constraints.append(self.state_set.contains(state_k)) 232 | constraints.append(self.input_set.contains(input_k)) 233 | 234 | state_k = state_k_next 235 | 236 | # 另一种构建优化问题的思路，只把输入序列当作决策变量 = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = 237 | # 238 | # constraints = [] 239 | # state_k = self.__real_time_state 240 | # 241 | # for k in range(self.__pred_horizon): 242 | # input_k = self.__input_series[k * self.input_dim:(k + 1) * self.input_dim] 243 | # 244 | # cost = cost + (state_k @ self.q @ state_k + input_k @ self.r @ input_k) 245 | # constraints.append(state_set.is_interior_point(state_k) <= 0) 246 | # constraints.append(input_set.is_interior_point(input_k) <= 0) 247 | # 248 | # state_k = self.a @ state_k + self.b @ input_k 249 | # 250 | # = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = 251 | 252 | cost = cost + (state_k @ self.p @ state_k) / 2 253 | if self.__terminal_set_type == "zero": 254 | constraints.append(state_k == 0) 255 | else: 256 | constraints.append(self.terminal_set.contains(state_k)) 257 | 258 | problem = cp.Problem(cp.Minimize(cost), constraints) 259 | 260 | return problem 261 | 262 | @property 263 | @cache 264 | def feasible_set(self) -> Polyhedron: 265 | # 这里先求出了M，C矩阵，于是知道了Xk = M*x + C*Uk，之后将约束条件转化为G*Xk <= h，再包含A_Uk*Uk <= b_Uk 266 | # 于是有 267 | # [G*M G*C ][x ] [h ] 268 | # [ ][ ] <= [ ] 269 | # [ 0 A_Uk][Uk] [b_Uk] 270 | # a_bar, b_bar分别代表上面两个矩阵 271 | 272 | if self.__terminal_set_type == "ellipsoid": 273 | raise MPCNotImplementedException("calculation for the feasible set when the terminal set is a ellipsoid") 274 | # 生成矩阵 M 275 | m = np.zeros((self.state_dim * (self.__pred_horizon + 1), self.state_dim)) 276 | a_i = np.eye(self.state_dim) 277 | 278 | for i in range(self.pred_horizon + 1): 279 | m[i * self.state_dim : (i + 1) * self.state_dim, :] = a_i 280 | a_i = a_i @ self.a 281 | 282 | # 生成矩阵 C 283 | c = spl.block_diag(*[self.b for _ in range(self.__pred_horizon)]) 284 | 285 | for i in range(self.__pred_horizon - 1): 286 | c[(i + 1) * self.state_dim : (i + 2) * self.state_dim, :] += ( 287 | self.a @ c[i * self.state_dim : (i + 1) * self.state_dim, :] 288 | ) 289 | 290 | zero = np.zeros((self.state_dim, self.input_dim * self.__pred_horizon)) 291 | c = np.vstack((zero, c)) 292 | 293 | # 生成 G 和 h 294 | g = spl.block_diag(*[self.state_set.l_mat for _ in range(self.__pred_horizon)], self.terminal_set.l_mat) 295 | h = np.hstack((np.tile(self.state_set.r_vec, self.pred_horizon), self.terminal_set.r_vec)) 296 | 297 | # 生成 A_Uk 和 b_Uk 298 | a_uk = spl.block_diag(*[self.input_set.l_mat for _ in range(self.__pred_horizon)]) 299 | b_uk = np.tile(self.input_set.r_vec, self.__pred_horizon) 300 | 301 | # 生成对应的零矩阵部分 302 | zero = np.zeros((a_uk.shape[0], self.state_dim)) 303 | 304 | l_mat = np.block([[g @ m, g @ c], [zero, a_uk]]) 305 | r_vec = np.hstack((h, b_uk)) 306 | 307 | feasible_set = Polyhedron(l_mat, r_vec) 308 | 309 | # 傅里叶-莫茨金消元法，将控制输入变量U消去 310 | feasible_set.fourier_motzkin_elimination(self.input_dim * self.__pred_horizon) 311 | 312 | return feasible_set 313 | -------------------------------------------------------------------------------- /src/tmpc/mpc/exception.py: -------------------------------------------------------------------------------- 1 | class MPCTypeException(Exception): 2 | def __init__(self, name: str, tp: str): 3 | message = "The type of " + name + " must be " + tp + "!" 4 | super().__init__(message) 5 | 6 | 7 | class MPCDimensionException(Exception): 8 | def __init__(self, *obj: str): 9 | message = "The dimensions of " + ", ".join(obj) + " do not match!" 10 | super().__init__(message) 11 | 12 | 13 | class MPCTerminalSetTypeException(Exception): 14 | def __init__(self): 15 | super().__init__("The terminal set type must be 'zero', 'ellipsoid' or 'polyhedron'") 16 | 17 | 18 | class MPCNotImplementedException(Exception): 19 | def __init__(self, function: str): 20 | message = "The function " + function + " has not been implemented yet!" 21 | super().__init__(message) 22 | -------------------------------------------------------------------------------- /src/tmpc/mpc/mpc.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cvxpy as cp 3 | from .base import MPCBase 4 | from .exception import * 5 | from ..set import Polyhedron 6 | 7 | 8 | class MPC(MPCBase): 9 | def __init__( 10 | self, 11 | a: np.ndarray, 12 | b: np.ndarray, 13 | q: np.ndarray, 14 | r: np.ndarray, 15 | pred_horizon: int, 16 | state_set: Polyhedron, 17 | input_set: Polyhedron, 18 | terminal_set_type="polyhedron", 19 | solver=cp.OSQP, 20 | ): 21 | super().__init__(a, b, q, r, pred_horizon, terminal_set_type, solver) 22 | 23 | if not (self.state_dim == state_set.n_dim): 24 | raise MPCDimensionException("state set", "state in controller") 25 | if not (self.input_dim == input_set.n_dim): 26 | raise MPCDimensionException("input set", "input in controller") 27 | 28 | self.__state_set = state_set 29 | self.__input_set = input_set 30 | 31 | @property 32 | def state_set(self) -> Polyhedron: 33 | return self.__state_set 34 | 35 | @property 36 | def input_set(self) -> Polyhedron: 37 | return self.__input_set 38 | 39 | @property 40 | def initial_constraint(self): 41 | return self.state_ini - self.real_time_state == 0 42 | 43 | def __call__(self, real_time_state: np.ndarray) -> np.ndarray: 44 | self.real_time_state = real_time_state 45 | self.problem.solve(solver=self.solver) 46 | 47 | return self.input_ini.value 48 | -------------------------------------------------------------------------------- /src/tmpc/mpc/tube_based_mpc.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cvxpy as cp 3 | from functools import cache 4 | from .base import MPCBase 5 | from ..set import Polyhedron, support_fun, unit_cube 6 | from .exception import * 7 | 8 | 9 | class TubeBasedMPC(MPCBase): 10 | def __init__( 11 | self, 12 | a: np.ndarray, 13 | b: np.ndarray, 14 | q: np.ndarray, 15 | r: np.ndarray, 16 | pred_horizon: int, 17 | state_set: Polyhedron, 18 | input_set: Polyhedron, 19 | noise_set: Polyhedron, 20 | terminal_set_type="polyhedron", 21 | solver=cp.PIQP, 22 | ): 23 | super().__init__(a, b, q, r, pred_horizon, terminal_set_type, solver) 24 | 25 | if not (self.state_dim == noise_set.n_dim): 26 | raise MPCDimensionException("noise set and state in controller!") 27 | 28 | self.__noise_set = noise_set 29 | 30 | self.__tightened_state_set = state_set - self.disturbance_invariant_set 31 | self.__tightened_input_set = input_set - self.k @ self.disturbance_invariant_set 32 | 33 | def __call__(self, real_time_state: np.ndarray) -> np.ndarray: 34 | self.real_time_state = real_time_state 35 | self.problem.solve(solver=self.solver) 36 | 37 | return self.input_ini.value - self.k @ (real_time_state - self.state_ini.value) 38 | 39 | @property 40 | def noise_set(self) -> Polyhedron: 41 | return self.__noise_set 42 | 43 | @property 44 | def state_set(self) -> Polyhedron: 45 | return self.__tightened_state_set 46 | 47 | @property 48 | def input_set(self) -> Polyhedron: 49 | return self.__tightened_input_set 50 | 51 | @property 52 | def initial_constraint(self) -> cp.Constraint: 53 | return self.disturbance_invariant_set.contains(self.real_time_state - self.state_ini) 54 | 55 | @property 56 | @cache 57 | def disturbance_invariant_set(self, alpha=0.2, epsilon=0.001) -> Polyhedron: 58 | alp = alpha 59 | 60 | # 由于多次给集合左乘 A_k，且 A_k 可逆，可以提前求好 A_k 的逆并在下面的计算 1、计算 2 中右乘 A_k 的逆，这里为了方便理解，没有这么做 61 | # a_k_inv = npl.inv(self.a - self.b @ self.k) 62 | a_k = self.a - self.b @ self.k 63 | 64 | a_k_s = np.eye(self.state_dim) 65 | a_k_s_noise_set = self.__noise_set 66 | sum_a_k_s_noise_set = self.__noise_set 67 | alpha_noise_set = alp * self.__noise_set 68 | 69 | while True: 70 | if a_k_s_noise_set.subset_eq(alpha_noise_set): 71 | break 72 | 73 | a_k_s = a_k @ a_k_s 74 | 75 | # 计算 1 - - - - - - - - - - - - - - - - - - - # 76 | # a_k_s_noise_set = a_k_s_noise_set @ a_k_inv 77 | a_k_s_noise_set = a_k @ a_k_s_noise_set 78 | # - - - - - - - - - - - - - - - - - - - - - - # 79 | 80 | sum_a_k_s_noise_set = sum_a_k_s_noise_set + a_k_s_noise_set 81 | 82 | alp = max( 83 | [ 84 | support_fun(self.__noise_set.l_mat[i, :], a_k_s_noise_set) / a_k_s_noise_set.r_vec[i] 85 | for i in range(self.__noise_set.n_edges) 86 | ] 87 | ) 88 | 89 | f_alpha_s_set = sum_a_k_s_noise_set / (1 - alp) 90 | 91 | a_k_n = a_k_s 92 | a_k_n_noise_set = a_k_s_noise_set 93 | sum_a_k_n_noise_set = sum_a_k_s_noise_set 94 | # 这里用一个单位球的内接超正方体代替单位球 95 | unit_cube_ = unit_cube(self.state_dim, 2 * epsilon / np.sqrt(self.state_dim)) 96 | 97 | while True: 98 | a_k_n = a_k @ a_k_n 99 | 100 | if a_k_n_noise_set.subset_eq(unit_cube_): 101 | break 102 | 103 | # 计算 2 - - - - - - - - - - - - - - - - - - - # 104 | # a_k_n_noise_set = a_k_n_noise_set @ a_k_inv 105 | a_k_n_noise_set = a_k @ a_k_n_noise_set 106 | # - - - - - - - - - - - - - - - - - - - - - - # 107 | 108 | sum_a_k_n_noise_set = sum_a_k_n_noise_set + a_k_n_noise_set 109 | 110 | disturbance_invariant_set = a_k_n @ f_alpha_s_set + sum_a_k_n_noise_set 111 | 112 | return disturbance_invariant_set 113 | -------------------------------------------------------------------------------- /src/tmpc/set/__init__.py: -------------------------------------------------------------------------------- 1 | from .base import support_fun 2 | from .poly import Polyhedron, rn, unit_cube 3 | from .ellipsoid import Ellipsoid 4 | -------------------------------------------------------------------------------- /src/tmpc/set/base.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cvxpy as cp 3 | import matplotlib.pyplot as plt 4 | import abc 5 | from typing import Union 6 | from .exception import * 7 | 8 | 9 | class SetBase(metaclass=abc.ABCMeta): 10 | # 集合的维度 11 | @property 12 | @abc.abstractmethod 13 | def n_dim(self) -> int: ... 14 | 15 | # 判断是否为内点，同时也可以作为cvxpy求解器接口 16 | @abc.abstractmethod 17 | def contains(self, point: Union[np.ndarray, cp.Expression]) -> Union[bool, cp.Constraint]: ... 18 | 19 | # 判断一个集合是否被包含于另一个集合 20 | @abc.abstractmethod 21 | def subset_eq(self, other: "SetBase"): ... 22 | 23 | # 画图（仅实现二维画图） 24 | @abc.abstractmethod 25 | def plot(self, ax: plt.Axes, n_points=2000, color="b") -> None: ... 26 | 27 | # 闵可夫斯基和（或平移） 28 | @abc.abstractmethod 29 | def __add__(self, other: Union["SetBase", np.ndarray]) -> "SetBase": ... 30 | 31 | # 庞特里亚金差，即闵可夫斯基和的逆运算 32 | # 即若 p2 = p1 + p3，则 p3 = p2 - p1，只有当输入为一个点（数组）时该运算等价于 (-p1) + p2 33 | @abc.abstractmethod 34 | def __sub__(self, other: Union["SetBase", np.ndarray]) -> "SetBase": ... 35 | 36 | # 多面体坐标变换，Set_new = Set @ mat 意味着 Set 是将 Set_new 中的所有点通过 mat 映射后的区域，这一定义是为了方便计算不变集 37 | @abc.abstractmethod 38 | def __matmul__(self, other: np.ndarray) -> "SetBase": ... 39 | 40 | # 多面体坐标变换，Set_new = Set @ mat 意味着 Set_new 是将 Poly 中的所有点通过 mat 映射后的区域，这一定义是为了方便计算不变集 41 | @abc.abstractmethod 42 | def __array_ufunc__(self, ufunc, method, *inputs, **kwargs) -> "SetBase": ... 43 | 44 | # 集合的放缩 45 | @abc.abstractmethod 46 | def __mul__(self, other: Union[int, float]) -> "SetBase": ... 47 | 48 | def __rmul__(self, other: Union[int, float]) -> "SetBase": 49 | return self.__mul__(other) 50 | 51 | def __truediv__(self, other: Union[int, float]) -> "SetBase": 52 | return self.__mul__(1 / other) 53 | 54 | # 集合取交集 55 | @abc.abstractmethod 56 | def __and__(self, other: "SetBase") -> "SetBase": ... 57 | 58 | # 判断两个集合是否相等 59 | @abc.abstractmethod 60 | def __eq__(self, other: "SetBase") -> bool: ... 61 | 62 | 63 | def support_fun(eta: np.ndarray, s: SetBase) -> Union[int, float]: 64 | if eta.ndim != 1: 65 | raise SetTypeException("input 'eta'", "support function", "1D array") 66 | if eta.size != s.n_dim: 67 | raise SetDimensionException("'eta'", "'polyhedron'") 68 | 69 | var = cp.Variable(s.n_dim) 70 | prob = cp.Problem(cp.Maximize(eta @ var), [s.contains(var)]) 71 | prob.solve(solver=cp.GLPK) 72 | 73 | return prob.value 74 | -------------------------------------------------------------------------------- /src/tmpc/set/ellipsoid.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import numpy.linalg as npl 3 | import cvxpy as cp 4 | import matplotlib.pyplot as plt 5 | from typing import Union 6 | from .base import SetBase 7 | from .exception import * 8 | 9 | 10 | class Ellipsoid(SetBase): 11 | def __init__(self, p: np.ndarray, alpha: Union[int, float], center: np.ndarray = None): 12 | try: 13 | _ = npl.cholesky(p) 14 | except npl.LinAlgError: 15 | raise SetTypeException("'P' matrix", "ellipsoid", "positive definite matrix") 16 | 17 | self.__p = p 18 | self.__n_dim = p.shape[0] 19 | self.__alpha = alpha 20 | 21 | self.__center = np.zeros(self.__n_dim) if center is None else center 22 | 23 | def __str__(self) -> str: 24 | return ( 25 | "====================================================================================================\n" 26 | "(x - center).T @ p @ (x - center) <= alpha\n" 27 | "====================================================================================================\n" 28 | f"p:\n" 29 | f"{self.__p}\n" 30 | "----------------------------------------------------------------------------------------------------\n" 31 | f"alpha:\n" 32 | f"{self.__alpha}\n" 33 | "----------------------------------------------------------------------------------------------------\n" 34 | f"center:\n" 35 | f"{self.__center}\n" 36 | "====================================================================================================" 37 | ) 38 | 39 | def contains(self, point: Union[np.ndarray, cp.Expression]) -> Union[bool, cp.Constraint]: 40 | if isinstance(point, np.ndarray): 41 | res = np.all((point - self.__center) @ self.__p @ (point - self.__center) - self.__alpha <= 0) 42 | else: 43 | res = cp.quad_form(point - self.__center, self.__p) - self.__alpha <= 0 44 | 45 | return res 46 | 47 | def subset_eq(self, other: "Ellipsoid") -> bool: 48 | raise SetNotImplementedException("subset_eq", "ellipsoid") 49 | 50 | def plot(self, ax: plt.Axes, n_points=2000, color="b") -> None: 51 | if self.__n_dim != 2: 52 | raise SetPlotException() 53 | 54 | axis_max = np.sqrt(self.__alpha / npl.eigvals(self.__p)) 55 | x_max, y_max = axis_max * 1.5 56 | x_min, y_min = -axis_max * 1.5 57 | 58 | x = np.linspace(x_min, x_max, n_points) 59 | y = np.linspace(y_min, y_max, n_points) 60 | x_grid, y_grid = np.meshgrid(x, y) 61 | x_grid = x_grid - self.__center[0] 62 | y_grid = y_grid - self.__center[1] 63 | 64 | z = ( 65 | x_grid**2 * self.__p[0, 0] 66 | + x_grid * y_grid * (self.__p[0, 1] + self.__p[1, 0]) 67 | + y_grid**2 * self.__p[1, 1] 68 | ) 69 | 70 | ax.contour(x_grid, y_grid, z, levels=[self.__alpha], colors=color) 71 | 72 | @property 73 | def p(self) -> np.ndarray: 74 | return self.__p 75 | 76 | @property 77 | def n_dim(self) -> int: 78 | return self.__n_dim 79 | 80 | @property 81 | def alpha(self) -> Union[int, float]: 82 | return self.__alpha 83 | 84 | @property 85 | def center(self) -> np.ndarray: 86 | return self.__center 87 | 88 | def __add__(self, other: Union["Ellipsoid", np.ndarray]) -> "Ellipsoid": 89 | if isinstance(other, Ellipsoid): 90 | raise SetNotImplementedException("pontryagin difference", "ellipsoid") 91 | else: 92 | return self.__class__(self.__p, self.__alpha, self.__center + other) 93 | 94 | def __sub__(self, other: Union["Ellipsoid", np.ndarray]) -> "Ellipsoid": 95 | if isinstance(other, Ellipsoid): 96 | raise SetNotImplementedException("pontryagin difference", "ellipsoid") 97 | else: 98 | return self.__add__(-other) 99 | 100 | def __matmul__(self, other: np.ndarray) -> "Ellipsoid": 101 | if other.ndim != 2: 102 | raise SetCalculationException("ellipsoid", "multiplied", "2D array") 103 | if other.shape[0] != self.__n_dim: 104 | raise SetCalculationException("ellipsoid", "multiplied", "array with matching dimension") 105 | 106 | return self.__class__(other.T @ self.__p @ other, self.__alpha, self.__center) 107 | 108 | def __array_ufunc__(self, ufunc, method, *inputs, **kwargs) -> "Ellipsoid": 109 | if ufunc == np.matmul: 110 | lhs, rhs = inputs 111 | try: 112 | res = self.__matmul__(npl.inv(lhs)) 113 | except npl.LinAlgError: 114 | res = NotImplemented 115 | elif ufunc == np.add: 116 | lhs, rhs = inputs 117 | res = self.__add__(lhs) 118 | else: 119 | res = NotImplemented 120 | 121 | return res 122 | 123 | # 多面体的放缩 124 | def __mul__(self, other: Union[int, float]) -> "Ellipsoid": 125 | if other < 0: 126 | raise SetCalculationException("ellipsoid", "multiplied", "positive number") 127 | 128 | return self.__class__(self.__p, self.__alpha * other, self.__center) 129 | 130 | def __and__(self, other: "Ellipsoid") -> "Ellipsoid": 131 | raise SetNotImplementedException("intersection", "ellipsoid") 132 | 133 | def __eq__(self, other: "Ellipsoid") -> bool: 134 | return (self.__center == other.__center) and np.all((self.__p / other.__p) == (self.__alpha / other.__alpha)) 135 | -------------------------------------------------------------------------------- /src/tmpc/set/exception.py: -------------------------------------------------------------------------------- 1 | class SetTypeException(Exception): 2 | def __init__(self, name: str, set_type: str, tp: str): 3 | message = "The type of the " + name + " of " + set_type + " must be " + tp + "!" 4 | super().__init__(message) 5 | 6 | 7 | class SetDimensionException(Exception): 8 | def __init__(self, *args: str): 9 | message = "The dimensions of " + ", ".join(args) + "do not match!" 10 | super().__init__(message) 11 | 12 | 13 | class SetCalculationException(Exception): 14 | def __init__(self, set_type: str, operation: str, other: str): 15 | message = "A " + set_type + " can only be " + operation + " by a " + other + "!" 16 | super().__init__(message) 17 | 18 | 19 | class SetNotImplementedException(Exception): 20 | def __init__(self, function: str, set_type: str): 21 | message = "The function " + function + " of " + set_type + " has not been implemented yet!" 22 | super().__init__(message) 23 | 24 | 25 | class SetPlotException(Exception): 26 | def __init__(self): 27 | super().__init__("Only 2D set can be plotted!") 28 | -------------------------------------------------------------------------------- /src/tmpc/set/poly.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import numpy.linalg as npl 3 | import cvxpy as cp 4 | import matplotlib.pyplot as plt 5 | from typing import Union, List 6 | from .base import SetBase, support_fun 7 | from .exception import * 8 | from .ellipsoid import Ellipsoid 9 | 10 | 11 | class InscribedEllipsoidException(Exception): 12 | def __init__(self): 13 | super().__init__("Cannot find a maximum inscribed ellipsoid since the center is not in the polyhedron!") 14 | 15 | 16 | class Polyhedron(SetBase): 17 | # 用线性不等式组 A @ x <= b 来表示一个多边形 18 | # n_edges: 边的个数 19 | # n_dim: 多面体维度 20 | # l_mat: A 21 | # r_vec: b 22 | 23 | def __init__(self, l_mat: np.ndarray, r_vec: np.ndarray): 24 | if l_mat.ndim != 2: 25 | raise SetTypeException("left matrix", "polyhedron", "2D array") 26 | if r_vec.ndim != 1: 27 | raise SetTypeException("right vector", "polyhedron", "1D array") 28 | if l_mat.shape[0] != r_vec.shape[0]: 29 | raise SetDimensionException("left matrix", "right vector") 30 | 31 | self.__n_edges, self.__n_dim = l_mat.shape 32 | self.__l_mat = l_mat 33 | self.__r_vec = r_vec 34 | 35 | @property 36 | def n_edges(self) -> int: 37 | return self.__n_edges 38 | 39 | @property 40 | def n_dim(self) -> int: 41 | return self.__n_dim 42 | 43 | @property 44 | def l_mat(self) -> np.ndarray: 45 | return self.__l_mat 46 | 47 | @property 48 | def r_vec(self) -> np.ndarray: 49 | return self.__r_vec 50 | 51 | # 浅拷贝 52 | def __copy__(self, other: "Polyhedron") -> None: 53 | self.__n_edges = other.__n_edges 54 | self.__n_dim = other.__n_dim 55 | self.__l_mat = other.__l_mat 56 | self.__r_vec = other.__r_vec 57 | 58 | # 打印该多面体的信息 59 | def __str__(self) -> str: 60 | return ( 61 | "====================================================================================================\n" 62 | "left matrix @ x <= right vector\n" 63 | "====================================================================================================\n" 64 | f"left matrix:\n" 65 | f"{self.__l_mat}\n" 66 | "----------------------------------------------------------------------------------------------------\n" 67 | f"right vector:\n" 68 | f"{self.__r_vec}\n" 69 | "====================================================================================================" 70 | ) 71 | 72 | def contains(self, point: Union[np.ndarray, cp.Expression]) -> Union[bool, cp.Constraint]: 73 | if isinstance(point, np.ndarray): 74 | res = np.all(self.__l_mat @ point - self.__r_vec <= 0) 75 | else: 76 | res = self.__l_mat @ point - self.__r_vec <= 0 77 | 78 | return res 79 | 80 | # 多边形绘图会有误差，因为采用等高线来画的，增加采样点的个数可以提高画图精度 81 | def plot( 82 | self, 83 | ax: plt.Axes, 84 | x_lim: List[Union[int, float]] = None, 85 | y_lim: List[Union[int, float]] = None, 86 | default_bound=100, 87 | n_points=2000, 88 | color="b", 89 | ) -> None: 90 | if self.__n_dim != 2: 91 | raise SetPlotException() 92 | if x_lim is None: 93 | x_min = -support_fun(np.array([-1, 0]), self) 94 | x_max = support_fun(np.array([1, 0]), self) 95 | x_lim = self.get_grid_lim(x_min, x_max, default_bound) 96 | if y_lim is None: 97 | y_min = -support_fun(np.array([0, -1]), self) 98 | y_max = support_fun(np.array([0, 1]), self) 99 | y_lim = self.get_grid_lim(y_min, y_max, default_bound) 100 | 101 | x = np.linspace(*x_lim, n_points) 102 | y = np.linspace(*y_lim, n_points) 103 | x_grid, y_grid = np.meshgrid(x, y) 104 | 105 | z = np.sum( 106 | np.maximum( 107 | self.__l_mat[:, 0, np.newaxis, np.newaxis] * x_grid 108 | + self.__l_mat[:, 1, np.newaxis, np.newaxis] * y_grid 109 | - self.__r_vec[:, np.newaxis, np.newaxis], 110 | 0, 111 | ), 112 | axis=0, 113 | ) 114 | 115 | ax.contour(x_grid, y_grid, z, levels=0, colors=color) 116 | 117 | # 绘制一个多面体时需要知道大概范围 118 | @staticmethod 119 | def get_grid_lim( 120 | val_min: Union[int, float], val_max: Union[int, float], default_bound: Union[int, float] 121 | ) -> List[Union[int, float]]: 122 | if val_min == -float("inf") and val_max == float("inf"): 123 | lim = [-default_bound, default_bound] 124 | elif val_min == -float("inf") and val_max != float("inf"): 125 | bound = abs(val_max) * 1.2 126 | lim = [-bound, bound] 127 | elif val_min != -float("inf") and val_max == float("inf"): 128 | bound = abs(val_min) * 1.2 129 | lim = [-bound, bound] 130 | else: 131 | margin = 0.1 * (val_max - val_min) 132 | lim = [val_min - margin, val_max + margin] 133 | 134 | return lim 135 | 136 | def subset_eq(self, other: "Polyhedron") -> bool: 137 | return all([support_fun(other.__l_mat[i, :], self) <= other.__r_vec[i] for i in range(other.__n_edges)]) 138 | 139 | # 将不等式组右侧向量归一化，防止系数过大，影响支撑函数（线性规划）求解 140 | def normalization(self) -> None: 141 | r_vec_abs = np.where(self.__r_vec == 0, 1, np.abs(self.__r_vec)) 142 | self.__l_mat = self.__l_mat / r_vec_abs[:, np.newaxis] 143 | self.__r_vec = self.__r_vec / r_vec_abs 144 | 145 | # 去除某个边 146 | def remove_edge(self, edge: Union[int, List]) -> "Polyhedron": 147 | l_mat = np.delete(self.__l_mat, edge, 0) 148 | r_vec = np.delete(self.__r_vec, edge, 0) 149 | 150 | return self.__class__(l_mat, r_vec) 151 | 152 | # 去除冗余项 153 | def remove_redundant_term(self) -> None: 154 | # 一条边不可能冗余 155 | if self.__n_edges < 2: 156 | return 157 | 158 | for i in range(self.__n_edges - 1, 0, -1): 159 | without_row_i = self.remove_edge(i) 160 | s_i = self.__r_vec[i] - support_fun(self.__l_mat[i, :], without_row_i) 161 | 162 | if s_i >= 0: 163 | self.__copy__(without_row_i) 164 | 165 | # 傅里叶-莫茨金消元法，这里从最后一个元素开始倒着消除，因此使用该方法前应该把需要保留的元素放在最前面 166 | # 相当于给一个变量左乘矩阵 167 | # [1 0 0 ... 0 0 ... 0] 168 | # [0 1 0 ... 0 0 ... 0] 169 | # [. . . ... . . ... .] 170 | # [0 0 0 ... 1 0 ... 0] 171 | # | --- | 172 | # | 173 | # V 174 | # 减少的维度 175 | def fourier_motzkin_elimination(self, n_dim: int) -> None: 176 | if n_dim < 0: 177 | raise SetTypeException("eliminated dimension", "polyhedron", "positive integer") 178 | for _ in range(n_dim): 179 | pos_a = np.empty((0, self.__n_dim - 1)) 180 | pos_b = np.empty(0) 181 | neg_a = np.empty((0, self.__n_dim - 1)) 182 | neg_b = np.empty(0) 183 | 184 | new_a = np.empty((0, self.__n_dim - 1)) 185 | new_b = np.empty(0) 186 | 187 | for i in range(self.__n_edges): 188 | a_i_last = self.__l_mat[i, -1] 189 | a_i_others = self.__l_mat[i, :-1] 190 | b_i = self.__r_vec[i] 191 | 192 | if a_i_last > 0: 193 | pos_a = np.vstack((pos_a, a_i_others / a_i_last)) 194 | pos_b = np.hstack((pos_b, b_i / a_i_last)) 195 | elif a_i_last == 0: 196 | new_a = np.vstack((new_a, a_i_others)) 197 | new_b = np.hstack((new_b, b_i)) 198 | else: 199 | neg_a = np.vstack((neg_a, -a_i_others / a_i_last)) 200 | neg_b = np.hstack((neg_b, -b_i / a_i_last)) 201 | 202 | for i in range(pos_a.shape[0]): 203 | for j in range(neg_a.shape[0]): 204 | new_a = np.vstack((new_a, pos_a[i, :] + neg_a[j, :])) 205 | new_b = np.hstack((new_b, pos_b[i] + neg_b[j])) 206 | 207 | self.__init__(new_a, new_b) 208 | self.remove_redundant_term() 209 | 210 | # 相当于给一个变量左乘矩阵 211 | # [1 0 0 ... 0] 212 | # [0 1 0 ... 0] 213 | # [. . . ... .] 214 | # [0 0 0 ... 1] 215 | # [0 0 0 ... 0] --- 216 | # [. . . ... .] | ---> 增加的维度 217 | # [0 0 0 ... 0] --- 218 | def extend_dimensions(self, n_dim: int) -> None: 219 | if n_dim < 0: 220 | raise SetTypeException("extended dimension", "polyhedron", "positive integer") 221 | elif n_dim > 0: 222 | zero_1 = np.zeros((self.__n_edges, n_dim)) 223 | zero_2 = np.zeros((n_dim, self.__n_dim)) 224 | zero_3 = np.zeros(2 * n_dim) 225 | eye = np.eye(n_dim) 226 | self.__n_edges = self.__n_edges + 2 * n_dim 227 | self.__n_dim = self.__n_dim + n_dim 228 | self.__l_mat = np.block([[self.__l_mat, zero_1], [zero_2, eye], [zero_2, -eye]]) 229 | self.__r_vec = np.block([self.__r_vec, zero_3]) 230 | 231 | # 闵可夫斯基和（或平移） 232 | def __add__(self, other: Union["Polyhedron", np.ndarray]) -> "Polyhedron": 233 | if isinstance(other, Polyhedron): 234 | h_self_other = np.array([support_fun(self.__l_mat[i, :], other) for i in range(self.__n_edges)]) 235 | h_other_self = np.array([support_fun(other.__l_mat[i, :], self) for i in range(other.__n_edges)]) 236 | 237 | res_l_mat = np.vstack((self.__l_mat, other.__l_mat)) 238 | res_r_vec = np.hstack((self.__r_vec + h_self_other, other.__r_vec + h_other_self)) 239 | 240 | res = self.__class__(res_l_mat, res_r_vec) 241 | res.remove_redundant_term() 242 | 243 | else: 244 | if other.ndim != 1: 245 | raise SetCalculationException("polyhedron", "added", "1D array") 246 | if other.size != self.__n_dim: 247 | raise SetCalculationException("polyhedron", "added", "array with matching dimension") 248 | 249 | res = self.__class__(self.__l_mat, self.__r_vec + self.__l_mat @ other) 250 | 251 | res.normalization() 252 | 253 | return res 254 | 255 | def __neg__(self) -> "Polyhedron": 256 | return self.__class__(-self.__l_mat, self.__r_vec) 257 | 258 | # 特别的，这里指庞特里亚金差，即闵可夫斯基和的逆运算 259 | # 即若 p2 = p1 + p3，则 p3 = p2 - p1，只有当输入为一个点（数组）时该运算等价于 (-p1) + p2 260 | def __sub__(self, other: Union["Polyhedron", np.ndarray]) -> "Polyhedron": 261 | if isinstance(other, Polyhedron): 262 | h_self_other = np.array([support_fun(self.__l_mat[i, :], other) for i in range(self.__n_edges)]) 263 | 264 | res = self.__class__(self.__l_mat, self.__r_vec - h_self_other) 265 | res.remove_redundant_term() 266 | res.normalization() 267 | 268 | else: 269 | res = self + (-other) 270 | 271 | return res 272 | 273 | # 多面体坐标变换，Poly_new = Poly @ mat 意味着 Poly 是将 Poly_new 中的所有点通过 mat 映射后的区域，这一定义是为了方便计算不变集 274 | def __matmul__(self, other: np.ndarray) -> "Polyhedron": 275 | if other.ndim != 2: 276 | raise SetCalculationException("polyhedron", "multiplied", "2D array") 277 | if other.shape[0] != self.__n_dim: 278 | raise SetCalculationException("polyhedron", "multiplied", "array with matching dimension") 279 | 280 | return self.__class__(self.__l_mat @ other, self.__r_vec) 281 | 282 | # 多面体坐标变换，Poly_new = Poly @ mat 意味着 Poly_new 是将 Poly 中的所有点通过 mat 映射后的区域，这一定义是为了方便计算不变集 283 | def __array_ufunc__(self, ufunc, method, *inputs, **kwargs) -> "Polyhedron": 284 | if ufunc == np.matmul: 285 | lhs, rhs = inputs 286 | row, col = lhs.shape 287 | u, s, v = npl.svd(lhs) 288 | rank = s.shape[0] 289 | 290 | res = self.__matmul__(v.T) 291 | res.fourier_motzkin_elimination(col - rank) 292 | res = res.__matmul__(np.diag(1 / s)) 293 | res.extend_dimensions(row - rank) 294 | res = res.__matmul__(u.T) 295 | elif ufunc == np.add: 296 | lhs, rhs = inputs 297 | res = self.__add__(lhs) 298 | else: 299 | res = NotImplemented 300 | 301 | return res 302 | 303 | def __mul__(self, other: Union[int, float]) -> "Polyhedron": 304 | if other < 0: 305 | raise SetCalculationException("polyhedron", "multiplied", "positive number") 306 | 307 | return self.__class__(self.__l_mat / other, self.__r_vec) 308 | 309 | def __and__(self, other: "Polyhedron") -> "Polyhedron": 310 | res = self.__class__(np.vstack((self.__l_mat, other.__l_mat)), np.hstack((self.__r_vec, other.__r_vec))) 311 | res.remove_redundant_term() 312 | 313 | return res 314 | 315 | def __eq__(self, other: "Polyhedron") -> bool: 316 | return self.subset_eq(other) and other.subset_eq(self) 317 | 318 | def get_max_ellipsoid(self, p: np.ndarray, center: np.ndarray = None) -> Ellipsoid: 319 | ellipsoid_center = np.zeros(self.__n_dim) if center is None else center 320 | 321 | if not self.contains(ellipsoid_center): 322 | raise InscribedEllipsoidException 323 | 324 | p_bar = npl.cholesky(p) 325 | r_vec_bar = self.__r_vec + self.__l_mat @ ellipsoid_center 326 | l_mat_bar = self.__l_mat @ npl.inv(p_bar).T 327 | min_center_to_edge_distance = np.min(np.abs(r_vec_bar) / npl.norm(l_mat_bar, ord=2, axis=1)) 328 | 329 | return Ellipsoid(p, min_center_to_edge_distance**2, center) 330 | 331 | 332 | def rn(dim: int): 333 | return Polyhedron(np.zeros((1, dim)), np.zeros(1)) 334 | 335 | 336 | def unit_cube(dim: int, side_length: Union[int, float]): 337 | if dim <= 0: 338 | raise SetTypeException("dimension", "unit cube", "positive integer") 339 | if side_length < 0: 340 | raise SetTypeException("side length", "unit cube", "non-negative real number") 341 | 342 | eye = np.eye(dim) 343 | 344 | return Polyhedron(np.vstack((eye, -eye)), (side_length / 2) * np.ones(2 * dim)) 345 | -------------------------------------------------------------------------------- /tests/lqr_and_linear_mpc.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import tmpc.set as ts 4 | import tmpc.mpc as tm 5 | 6 | if __name__ == "__main__": 7 | # Comparison of LQR, linear MPC = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = 8 | # Initialization - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 9 | N = 9 10 | x_dim = 2 11 | u_dim = 1 12 | 13 | A = np.array([[1, 1], [0, 1]]) 14 | B = np.array([[0.5], [1]]) 15 | Q = np.array([[1, 0], [0, 1]]) 16 | R = np.array([[0.01]]) 17 | 18 | A_x = np.array([[0, 1]]) 19 | b_x = np.array([2]) 20 | A_u = np.array([[1], [-1]]) 21 | b_u = np.array([1, 1]) 22 | 23 | x_set = ts.Polyhedron(A_x, b_x) 24 | u_set = ts.Polyhedron(A_u, b_u) 25 | 26 | lqr = tm.LQR(A, B, Q, R) 27 | mpc = tm.MPC(A, B, Q, R, N, x_set, u_set) 28 | 29 | terminal_set = mpc.terminal_set 30 | feasible_set = mpc.feasible_set 31 | 32 | # Simulation computation - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 33 | T = 20 34 | 35 | x_ini = np.array([20, -4]) 36 | 37 | # The input constraints are compulsory 38 | x_lqr = np.zeros((mpc.state_dim, T + 1)) 39 | u_lqr_nom = np.zeros((mpc.input_dim, T)) 40 | u_lqr_real = np.zeros((mpc.input_dim, T)) 41 | x_lqr[:, 0] = x_ini 42 | 43 | # Without consideration of input constraints 44 | # x_lqr = np.zeros((mpc_controller.state_dim, T + 1)) 45 | # u_lqr = np.zeros((mpc_controller.input_dim, T)) 46 | # x_lqr[:, 0] = x_ini 47 | 48 | x_mpc = np.zeros((mpc.state_dim, T + 1)) 49 | u_mpc = np.zeros((mpc.input_dim, T)) 50 | x_mpc[:, 0] = x_ini 51 | 52 | x_mpc_pred = np.zeros((T, x_dim, N + 1)) 53 | 54 | for k in range(T): 55 | w = np.random.uniform(-0.1, 0.1, x_dim) 56 | 57 | # When the input constraints are compulsory, use code as below 58 | u_lqr_nom[:, k] = lqr(x_lqr[:, k]) 59 | u_lqr_real[:, k] = np.clip(u_lqr_nom[:, k], -1, 1) 60 | x_lqr[:, k + 1] = A @ x_lqr[:, k] + B @ u_lqr_real[:, k] + w 61 | 62 | # Without consideration of input constraints 63 | # u_lqr[:, k] = lqr_controller(x_lqr[:, k]) 64 | # x_lqr[:, k + 1] = A @ x_lqr[:, k] + B @ u_lqr[:, k] + w 65 | 66 | u_mpc[:, k] = mpc(x_mpc[:, k]) 67 | x_mpc[:, k + 1] = A @ x_mpc[:, k] + B @ u_mpc[:, k] + w 68 | 69 | x_mpc_pred[k] = mpc.state_prediction_series 70 | 71 | # Results plot - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 72 | # State trajectory plot 73 | fig1, ax1 = plt.subplots(1, 1) 74 | ax1.grid(True) 75 | ax1.set_xlim([-10, 25]) 76 | ax1.set_ylim([-10, 5]) 77 | ax1.set_title("State trajectories of LQR and MPC") 78 | ax1.annotate( 79 | "State bound", xy=(2.5, 2), xytext=(2.5, 3), arrowprops=dict(arrowstyle="-|>") 80 | ) 81 | ax1.annotate( 82 | "Terminal set", xy=(1, -0.5), xytext=(5, -2), arrowprops=dict(arrowstyle="-|>") 83 | ) 84 | 85 | terminal_set.plot(ax1, color="k") 86 | x_set.plot(ax1, color="k") 87 | 88 | for k in range(T): 89 | l, u = (0, 1) if k == 0 else (k - 1, k + 1) 90 | 91 | (line_1,) = ax1.plot( 92 | x_lqr[0, l:u], 93 | x_lqr[1, l:u], 94 | color="g", 95 | marker="o", 96 | label="LQR real trajectory", 97 | ) 98 | 99 | (line_2,) = ax1.plot( 100 | x_mpc[0, l:u], 101 | x_mpc[1, l:u], 102 | color="b", 103 | marker="*", 104 | label="MPC real trajectory", 105 | ) 106 | (line_3,) = ax1.plot( 107 | x_mpc_pred[k][0, :], 108 | x_mpc_pred[k][1, :], 109 | color="r", 110 | marker="^", 111 | label="MPC predicted trajectory", 112 | ) 113 | 114 | if k == 0: 115 | ax1.legend() 116 | 117 | plt.pause(0.5) 118 | 119 | line_3.remove() 120 | 121 | plt.show() 122 | 123 | # Control input plot 124 | fig2, ax2 = plt.subplots(1, 1) 125 | ax2.set_title("Inputs of LQR and MPC") 126 | ax2.set_xlim([0, T - 1]) 127 | 128 | iterations = np.arange(T) 129 | 130 | # The input constraints are compulsory 131 | ax2.step(iterations, u_lqr_nom[0, :], label="LQR nominal input sequence") 132 | ax2.step(iterations, u_lqr_real[0, :], label="LQR real input sequence") 133 | 134 | # Without consideration of input constraints 135 | # ax2.step(iterations, u_lqr[0, :], label='LQR input sequence') 136 | 137 | ax2.step(iterations, u_mpc[0, :], label="MPC input sequence") 138 | 139 | ax2.step(iterations, np.ones(T) * 1, "y-.", label="Input bounds") 140 | ax2.step(iterations, np.ones(T) * -1, "y-.") 141 | 142 | ax2.legend(loc="upper right") 143 | 144 | # Feasible set of the initial state for MPC 145 | fig3, ax3 = plt.subplots(1, 1) 146 | ax3.grid(True) 147 | 148 | ax3.set_title("The feasible set of the initial state of MPC") 149 | 150 | ax3.plot(x_ini[0], x_ini[1], color="r", marker="*") 151 | ax3.annotate( 152 | "Initial state", 153 | xy=x_ini, 154 | xytext=x_ini + 0.3 * np.abs(x_ini), 155 | arrowprops=dict(arrowstyle="-|>"), 156 | ) 157 | feasible_set.plot(ax3) 158 | 159 | plt.show() 160 | -------------------------------------------------------------------------------- /tests/poly_test.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import tmpc.set as ts 4 | 5 | if __name__ == "__main__": 6 | # Test for set = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = 7 | fig1, ax1 = plt.subplots(1, 1) 8 | ax1.grid(True) 9 | ax1.axis("equal") 10 | ax1.set_title("Translation") 11 | 12 | p1 = ts.Polyhedron( 13 | np.array([[1, 0], [-1, 0], [0, 1], [0, -1]]), np.array([1, 0, 1, 0]) 14 | ) 15 | p2 = ts.Polyhedron(np.array([[-3, 1], [-1, -1], [1, 0]]), np.array([3, 1, 0])) 16 | p3 = p1 + np.array([0, 2]) 17 | 18 | p1.plot(ax1, color="b") 19 | p3.plot(ax1, color="r") 20 | 21 | fig2, ax2 = plt.subplots(1, 1) 22 | ax2.grid(True) 23 | ax2.axis("equal") 24 | ax2.set_title("Minkowski sum") 25 | 26 | p4 = p1 + p2 27 | 28 | p1.plot(ax2, color="b") 29 | p2.plot(ax2, color="r") 30 | p4.plot(ax2, color="g") 31 | 32 | fig3, ax3 = plt.subplots(1, 1) 33 | ax3.grid(True) 34 | ax3.axis("equal") 35 | ax3.set_title("Pontryagin difference") 36 | 37 | p5 = p4 - p1 38 | p6 = p4 + (-p1) 39 | 40 | p1.plot(ax3, color="b") 41 | p4.plot(ax3, color="r") 42 | p5.plot(ax3, color="g") 43 | p6.plot(ax3, color="k") 44 | 45 | fig4, ax4 = plt.subplots(1, 1) 46 | ax4.grid(True) 47 | ax4.axis("equal") 48 | ax4.set_title("Coordinate transformation") 49 | 50 | theta = np.deg2rad(90) 51 | s, c = np.sin(theta), np.cos(theta) 52 | rot_mat = np.array([[c, -s], [s, c]]) 53 | 54 | p7 = p2 @ rot_mat 55 | p8 = rot_mat @ p2 56 | 57 | p2.plot(ax4, color="b") 58 | p7.plot(ax4, color="r") 59 | p8.plot(ax4, color="g") 60 | 61 | fig5, ax5 = plt.subplots(1, 1) 62 | ax5.grid(True) 63 | ax5.axis("equal") 64 | ax5.set_title("$R^2$") 65 | 66 | R2 = ts.rn(2) 67 | 68 | R2.plot(ax5) 69 | 70 | fig6, ax6 = plt.subplots(1, 1) 71 | ax6.grid(True) 72 | ax6.axis("equal") 73 | ax6.set_title("Unit cube") 74 | 75 | unit_cube = ts.unit_cube(2, 1) 76 | 77 | unit_cube.plot(ax6) 78 | 79 | P = np.array([[1, -1], [-1, 3]]) 80 | e = p4.get_max_ellipsoid(P) 81 | 82 | fig7, ax7 = plt.subplots(1, 1) 83 | ax7.axis("equal") 84 | ax7.set_xlim([-2, 2]) 85 | ax7.grid(True) 86 | 87 | p4.plot(ax7, x_lim=[-2, 2], color="b") 88 | e.plot(ax7, color="r") 89 | 90 | results_path = "../results/poly_test/" 91 | 92 | fig1.savefig(results_path + "fig_1.png", dpi=300, bbox_inches="tight") 93 | fig2.savefig(results_path + "fig_2.png", dpi=300, bbox_inches="tight") 94 | fig3.savefig(results_path + "fig_3.png", dpi=300, bbox_inches="tight") 95 | fig4.savefig(results_path + "fig_4.png", dpi=300, bbox_inches="tight") 96 | fig5.savefig(results_path + "fig_5.png", dpi=300, bbox_inches="tight") 97 | fig6.savefig(results_path + "fig_6.png", dpi=300, bbox_inches="tight") 98 | fig7.savefig(results_path + "fig_7.png", dpi=300, bbox_inches="tight") 99 | -------------------------------------------------------------------------------- /tests/polyhedron_and_ellipsoid_terminal_set.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cvxpy as cp 3 | import matplotlib.pyplot as plt 4 | import tmpc.set as ts 5 | import tmpc.mpc as tm 6 | 7 | if __name__ == "__main__": 8 | # Comparison of LQR, linear MPC = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = 9 | # Initialization - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 10 | N = 9 11 | x_dim = 2 12 | u_dim = 1 13 | 14 | A = np.array([[1, 1], [0, 1]]) 15 | B = np.array([[0.5], [1]]) 16 | Q = np.array([[1, 0], [0, 1]]) 17 | R = np.array([[0.01]]) 18 | 19 | A_x = np.array([[0, 1]]) 20 | b_x = np.array([2]) 21 | A_u = np.array([[1], [-1]]) 22 | b_u = np.array([1, 1]) 23 | 24 | x_set = ts.Polyhedron(A_x, b_x) 25 | u_set = ts.Polyhedron(A_u, b_u) 26 | 27 | mpc_poly = tm.MPC(A, B, Q, R, N, x_set, u_set, terminal_set_type="polyhedron") 28 | mpc_elli = tm.MPC( 29 | A, B, Q, R, N, x_set, u_set, terminal_set_type="ellipsoid", solver=cp.ECOS 30 | ) 31 | 32 | poly_terminal_set = mpc_poly.terminal_set 33 | elli_terminal_set = mpc_elli.terminal_set 34 | 35 | # Simulation computation - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 36 | T = 15 37 | 38 | x_ini = np.array([20, -4]) 39 | # x_ini = np.array([32, -6]) 40 | 41 | # The input constraints are compulsory 42 | x_mpc_poly = np.zeros((mpc_poly.state_dim, T + 1)) 43 | u_mpc_poly = np.zeros((mpc_poly.input_dim, T)) 44 | x_mpc_poly[:, 0] = x_ini 45 | 46 | x_mpc_elli = np.zeros((mpc_elli.state_dim, T + 1)) 47 | u_mpc_elli = np.zeros((mpc_elli.input_dim, T)) 48 | x_mpc_elli[:, 0] = x_ini 49 | 50 | x_mpc_poly_pred = np.zeros((T, x_dim, N + 1)) 51 | x_mpc_elli_pred = np.zeros((T, x_dim, N + 1)) 52 | 53 | for k in range(T): 54 | w = np.random.uniform(-0.1, 0.1, x_dim) 55 | 56 | # When the input constraints are compulsory, use code as below 57 | u_mpc_poly[:, k] = mpc_poly(x_mpc_poly[:, k]) 58 | x_mpc_poly[:, k + 1] = A @ x_mpc_poly[:, k] + B @ u_mpc_poly[:, k] + w 59 | 60 | u_mpc_elli[:, k] = mpc_elli(x_mpc_elli[:, k]) 61 | x_mpc_elli[:, k + 1] = A @ x_mpc_elli[:, k] + B @ u_mpc_elli[:, k] + w 62 | 63 | x_mpc_poly_pred[k] = mpc_poly.state_prediction_series 64 | x_mpc_elli_pred[k] = mpc_elli.state_prediction_series 65 | 66 | # Results plot - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 67 | # State trajectory plot 68 | fig1, ax1 = plt.subplots(1, 1) 69 | ax1.grid(True) 70 | ax1.set_xlim([-5, 25]) 71 | ax1.set_ylim([-10, 5]) 72 | ax1.set_title("State trajectories") 73 | ax1.annotate( 74 | "State bound", xy=(2.5, 2), xytext=(2.5, 3), arrowprops=dict(arrowstyle="-|>") 75 | ) 76 | ax1.annotate( 77 | "Polyhedron terminal set", 78 | xy=(2, -1.2), 79 | xytext=(5, -2), 80 | arrowprops=dict(arrowstyle="-|>"), 81 | ) 82 | ax1.annotate( 83 | "Ellipsoid terminal set", 84 | xy=(0.2, -0.4), 85 | xytext=(5, 0), 86 | arrowprops=dict(arrowstyle="-|>"), 87 | ) 88 | 89 | poly_terminal_set.plot(ax1, color="k") 90 | elli_terminal_set.plot(ax1, color="k") 91 | x_set.plot(ax1, color="k") 92 | 93 | for k in range(T): 94 | l, u = (0, 1) if k == 0 else (k - 1, k + 1) 95 | 96 | (line_1,) = ax1.plot( 97 | x_mpc_poly[0, l:u], 98 | x_mpc_poly[1, l:u], 99 | color="b", 100 | marker="*", 101 | label="Poly real trajectory", 102 | ) 103 | (line_2,) = ax1.plot( 104 | x_mpc_poly_pred[k][0, :], 105 | x_mpc_poly_pred[k][1, :], 106 | color="r", 107 | marker="^", 108 | label="Poly predicted trajectory", 109 | ) 110 | 111 | (line_3,) = ax1.plot( 112 | x_mpc_elli[0, l:u], 113 | x_mpc_elli[1, l:u], 114 | color="g", 115 | marker="x", 116 | label="Elli real trajectory", 117 | ) 118 | (line_4,) = ax1.plot( 119 | x_mpc_elli_pred[k][0, :], 120 | x_mpc_elli_pred[k][1, :], 121 | color="y", 122 | marker="o", 123 | label="Elli predicted trajectory", 124 | ) 125 | 126 | if k == 0: 127 | ax1.legend() 128 | 129 | plt.pause(1) 130 | 131 | line_2.remove() 132 | line_4.remove() 133 | 134 | plt.show() 135 | 136 | # Control input plot 137 | fig2, ax2 = plt.subplots(1, 1) 138 | ax2.set_title("Input sequences") 139 | ax2.set_xlim([0, T - 1]) 140 | 141 | iterations = np.arange(T) 142 | 143 | # The input constraints are compulsory 144 | ax2.step(iterations, u_mpc_poly[0, :], label="Polyhedron") 145 | ax2.step(iterations, u_mpc_elli[0, :], label="Ellipsoid") 146 | 147 | ax2.step(iterations, np.ones(T) * 1, "y-.", label="Input bounds") 148 | ax2.step(iterations, np.ones(T) * -1, "y-.") 149 | 150 | ax2.legend(loc="upper right") 151 | 152 | plt.show() 153 | -------------------------------------------------------------------------------- /tests/tube_based_mpc.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import tmpc.set as ts 4 | import tmpc.mpc as tm 5 | 6 | if __name__ == "__main__": 7 | # Tube based MPC = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = 8 | # Initialization - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 9 | N = 9 10 | x_dim = 2 11 | u_dim = 1 12 | 13 | A = np.array([[1, 1], [0, 1]]) 14 | B = np.array([[0.5], [1]]) 15 | Q = np.array([[1, 0], [0, 1]]) 16 | R = np.array([[0.01]]) 17 | 18 | A_x = np.array([[0, 1]]) 19 | b_x = np.array([2]) 20 | A_u = np.array([[1], [-1]]) 21 | b_u = np.array([1, 1]) 22 | A_w = np.array([[1, 0], [-1, 0], [0, 1], [0, -1]]) 23 | b_w = np.array([0.1, 0.1, 0.1, 0.1]) 24 | 25 | x_set = ts.Polyhedron(A_x, b_x) 26 | u_set = ts.Polyhedron(A_u, b_u) 27 | w_set = ts.Polyhedron(A_w, b_w) 28 | 29 | # 各种集合的计算量较大，可能会花费较长时间 30 | t_mpc = tm.TubeBasedMPC(A, B, Q, R, N, x_set, u_set, w_set) 31 | 32 | disturbance_invariant_set = t_mpc.disturbance_invariant_set 33 | terminal_set = t_mpc.terminal_set 34 | terminal_set_plus_di = terminal_set + disturbance_invariant_set 35 | feasible_set_bar = t_mpc.feasible_set 36 | feasible_set = feasible_set_bar + disturbance_invariant_set 37 | 38 | # Simulation computation - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 39 | # Simulation time 40 | T = 9 41 | 42 | # Initial state 43 | x_ini = np.array([-5, -2]) 44 | 45 | # Record the real state and input during every iteration 46 | x_t_mpc = np.zeros((t_mpc.state_dim, T + 1)) 47 | u_t_mpc = np.zeros((t_mpc.input_dim, T)) 48 | u_nom_t_mpc = np.zeros((t_mpc.input_dim, T)) 49 | u_noise_t_mpc = np.zeros((t_mpc.input_dim, T)) 50 | x_t_mpc[:, 0] = x_ini 51 | 52 | # Record the predicted trajectory during every iteration 53 | x_t_mpc_pred = np.zeros((T, x_dim, N + 1)) 54 | 55 | for k in range(T): 56 | w = np.random.uniform(-0.1, 0.1, x_dim) 57 | 58 | u_t_mpc[:, k] = t_mpc(x_t_mpc[:, k]) # 实际输出 59 | u_nom_t_mpc[:, k] = t_mpc.input_ini.value # 名义输出 60 | u_noise_t_mpc[:, k] = -t_mpc.k @ ( 61 | x_t_mpc[:, k] - t_mpc.state_ini.value 62 | ) # 用于抑制噪声的输出 63 | x_t_mpc[:, k + 1] = A @ x_t_mpc[:, k] + B @ u_t_mpc[:, k] + w 64 | 65 | x_t_mpc_pred[k] = t_mpc.state_prediction_series 66 | 67 | # Test for disturbance invariant set 68 | T_test = 20 69 | x_test = np.zeros((x_dim, T_test + 1)) 70 | 71 | A_k = A - B @ t_mpc.k 72 | 73 | for k in range(T_test): 74 | w_test = np.random.uniform(-0.1, 0.1, x_dim) 75 | x_test[:, k + 1] = A_k @ x_test[:, k] + w_test 76 | 77 | # Results plot - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 78 | # State trajectory plot 79 | fig1, ax1 = plt.subplots(1, 1) 80 | ax1.set_title("State trajectory of tube based MPC") 81 | ax1.grid(True) 82 | ax1.set_xlim([-8, 4]) 83 | ax1.set_ylim([-3, 3]) 84 | 85 | terminal_set.plot(ax1, color="k") 86 | terminal_set_plus_di.plot(ax1, color="k") 87 | x_set.plot(ax1, color="k") 88 | 89 | ax1.annotate( 90 | "State bound", xy=(-6, 2), xytext=(-6, 2.5), arrowprops=dict(arrowstyle="-|>") 91 | ) 92 | ax1.annotate( 93 | "Terminal set", 94 | xy=(1.1, -0.3), 95 | xytext=(1.1, 0.5), 96 | arrowprops=dict(arrowstyle="-|>"), 97 | ) 98 | ax1.annotate( 99 | "Terminal set + Disturbance invariant set", 100 | xy=(0.5, -0.9), 101 | xytext=(-4, -2.5), 102 | arrowprops=dict(arrowstyle="-|>"), 103 | ) 104 | 105 | tube_text_pos = x_t_mpc_pred[3, :, 0] + np.array([0.25, -0.25]) 106 | ax1.annotate( 107 | "Tube", 108 | xy=tube_text_pos, 109 | xytext=tube_text_pos - 0.2 * tube_text_pos, 110 | arrowprops=dict(arrowstyle="-|>"), 111 | ) 112 | 113 | for k in range(T): 114 | tube = disturbance_invariant_set + x_t_mpc_pred[k, :, 0] 115 | tube.plot(ax1, color="k") 116 | 117 | for k in range(T): 118 | l, u = (0, 1) if k == 0 else (k - 1, k + 1) 119 | 120 | (line_1,) = ax1.plot( 121 | x_t_mpc[0, l:u], 122 | x_t_mpc[1, l:u], 123 | color="b", 124 | marker="*", 125 | label="MPC real trajectory", 126 | ) 127 | (line_2,) = ax1.plot( 128 | x_t_mpc_pred[l:u, 0, 0].reshape(-1), 129 | x_t_mpc_pred[l:u, 1, 0].reshape(-1), 130 | color="g", 131 | marker="s", 132 | label="MPC nominal trajectory", 133 | ) 134 | (line_3,) = ax1.plot( 135 | x_t_mpc_pred[k][0, :], 136 | x_t_mpc_pred[k][1, :], 137 | color="r", 138 | marker="^", 139 | label="MPC predicted trajectory", 140 | ) 141 | 142 | if k == 0: 143 | ax1.legend() 144 | 145 | plt.pause(1) 146 | 147 | line_3.remove() 148 | 149 | plt.show() 150 | 151 | # Control input plot 152 | fig2, ax2 = plt.subplots(3, 1, figsize=(6.4, 4.8 * 3)) 153 | fig2.suptitle("Inputs of the tube based MPC") 154 | 155 | iterations = np.arange(T) 156 | 157 | ax2[0].step(iterations, u_t_mpc[0, :], label="Tube based MPC input sequence") 158 | ax2[0].step(iterations, np.ones(T) * 1, "y--", label="Input bounds") 159 | ax2[0].step(iterations, np.ones(T) * -1, "y--") 160 | 161 | ax2[0].legend(loc="upper right") 162 | 163 | ax2[1].step(iterations, u_nom_t_mpc[0, :], label="Nominal input sequence") 164 | ax2[1].step(iterations, np.ones(T) * 1, "y--", label="Input bounds") 165 | ax2[1].step(iterations, np.ones(T) * -1, "y--") 166 | 167 | ax2[1].legend(loc="upper right") 168 | 169 | ax2[2].step( 170 | iterations, u_noise_t_mpc[0, :], label="Input sequence for noise reduction" 171 | ) 172 | ax2[2].step(iterations, np.ones(T) * 1, "y--", label="Input bounds") 173 | ax2[2].step(iterations, np.ones(T) * -1, "y--") 174 | 175 | ax2[2].legend(loc="upper right") 176 | 177 | # Feasible set of the initial state for MPC 178 | fig3, ax3 = plt.subplots(1, 1) 179 | ax3.set_title("Feasible set for the initial state of tube based MPC") 180 | ax3.grid(True) 181 | 182 | ax3.annotate( 183 | "Initial state", 184 | xy=x_ini, 185 | xytext=x_ini + 0.3 * np.abs(x_ini), 186 | arrowprops=dict(arrowstyle="-|>"), 187 | ) 188 | ax3.annotate( 189 | "Feasible set for initial state\nin controller", 190 | xy=(10, -4), 191 | xytext=(-20, -6), 192 | arrowprops=dict(arrowstyle="-|>"), 193 | ) 194 | ax3.annotate( 195 | "Feasible set for initial state", 196 | xy=(20, -5.8), 197 | xytext=(0, -8), 198 | arrowprops=dict(arrowstyle="-|>"), 199 | ) 200 | feasible_set_bar.plot(ax3, color="r") 201 | feasible_set.plot(ax3, color="b") 202 | 203 | ax3.plot(x_ini[0], x_ini[1], color="r", marker="*") 204 | 205 | plt.show() 206 | 207 | # State trajectory of the test for disturbance invariant set 208 | fig4, ax4 = plt.subplots(1, 1) 209 | ax4.set_title("State trajectory of the test for disturbance invariant set") 210 | ax4.grid(True) 211 | 212 | disturbance_invariant_set.plot(ax4, x_lim=[-0.3, 0.3], y_lim=[-0.3, 0.3], color="k") 213 | 214 | for k in range(T_test): 215 | l, u = (0, 1) if k == 0 else (k - 1, k + 1) 216 | ax4.plot( 217 | x_test[0, l:u], 218 | x_test[1, l:u], 219 | color="r", 220 | marker="*", 221 | label="Test state trajectory", 222 | ) 223 | 224 | if k == 0: 225 | pass 226 | 227 | plt.pause(1) 228 | 229 | plt.show() 230 | --------------------------------------------------------------------------------