Learning Model Predictive Control for Iterative Tasks. A Data-Driven Control Framework
一句话 MPC：
在每个采用点处，根据被控对象的状态和预测模型，预测系统在未来一段时间内的状态，依据某一性能指标（成本函数）来求解最优的一组控制序列，并将这组控制序列的第一个控制作用作为输出给执行机构，在下一个采样点继续执行优化算法。
一句话 （学习终端成本函数的）LMPC：
在已知系统动态模型的前提下，通过学习既往成功的轨迹，设计一条有限区间的控制序列，使得控制序列保证可行性的同时，控制成本不会随着迭代次数的增加而增加
Tips：
- nominal dynamic 理想模型
一、问题提出
Dynamic function 系统描述
我们有一个定常离散系统，初始状态为 $x_S$，平衡点为 $x_F$，其中 $f$ 连续：
$$
\begin{equation} x_{t+1}=f(x_t,u_t) \end{equation}
$$
$$
\begin{equation} x_t \in \mathcal {X},\ u_t\in \mathcal {U}\quad \ \forall t \geq 0. \end{equation}
$$
$$
\begin{equation} x_0^j = x_S\quad \ \forall j \geq 0. \end{equation}
$$
$$
\begin{equation} f(x_F,0)=x_F. \end{equation}
$$
我们提出一个这样的无限域的控制优化问题（在控制成本更低的前提下，使得控制效果更好）：
$$
\begin{align} J_{0\rightarrow \infty }^*(x_S)&=\min _{u_0,u_1,\ldots } \sum \limits {k=0}^{\infty } h(x_k,u_k)\ \text{s.t. }, &x=f(x_k,u_k)\quad \ \forall k\geq 0\ &x_0=x_S\ &x_k \in \mathcal {X},\ u_k \in \mathcal {U}\quad \ \forall k\geq 0 \end{align}
$$
其中，h 正定
$$
\begin{align} h(x_F,0) = 0\ \text {and}\ h(x_tj,u_tj) \succ 0 \ \forall \ x_t^j \in & {\mathbb {R}}^n \setminus \lbrace x_F\rbrace,\nonumber u_t^j \in {\mathbb {R}}^m\setminus \lbrace 0\rbrace \end{align}
$$
我们将此问题的最优解定义为为如下形式：
$$
\begin{align} {{\mathbf x}}^{} \ &= \ [{{x}}{*}_0, {{x}}{}_1,\ \ldots,\ {{x}}^{}_t,\ \ldots ]\nonumber\ {{\mathbf u}}^{} \ &= \ [{{u}}{*}_0, {{u}}{}_1,\ \ldots,\ {{u}}^{}_t,\ \ldots ]. \end{align}
$$
但是，无限域的优化问题计算量太大，甚至算不出最优解，所以我们将通过一个有限域的最优化问题求解。但预测区间的缩短会带来诸多不良影响，所以，我们通过数据驱动的方式保证控制效果。
我们学习的数据为系统每一次迭代的控制输入序列及其对应的系统状态。我们将系统第 $j$ 次迭代的控制输入及状态轨迹定义如下，下标 $t$ 表示第 $j$ 次迭代的 $t$ 时刻
$$
\begin{align} {{\mathbf u}}^j &= [u_0j, u_1j,\ \ldots,\ u_t^j,\ \ldots ]\
{{\mathbf x}}^j &= [x_0j, x_1j,\ \ldots,\ x_t^j,\ \ldots ] \end{align}
$$
二、LMPC 构建
Sampled Safe Set 采样安全集的构建
我们将收敛到平衡点 $x_F$ 的状态轨迹 ${\mathbf x}^j$ 中的状态点收集起来，组成一个采样安全集 $\mathcal {SS}$，其数学定义如下：
$$
\begin{equation} \mathcal {SS}^j = \text {}\left\lbrace \bigcup _{i \in M^j} \bigcup _{t=0}^{\infty } x_t^i \right\rbrace \end{equation}
$$
$$
\begin{equation} M^j = \text {} \Big \lbrace k \in [0,j]: \lim _{t \rightarrow \infty } x_t^k = x_F \Big \rbrace. \end{equation}
$$
Iteration cost 迭代成本的构建
我们为 $\mathcal {SS}$ 集中每一个状态 $x_t^j$ 给定一个 cost-to-go 成本，定义为从当前状态 $x_t^j$ 出发，通过相应的控制输入序列 u，需要多少控制成本，使得最终的状态收敛至 $x_F$ ，公式表示如下
$$
\begin{equation} J_{t\rightarrow \infty }j(x_tj) = \ \sum \limits _{k=t}^{\infty } h(x_kj,u_kj) \end{equation}
$$
接着，我们定义终端成本函数 $Q^j(x)$ 为：从历史数据中得知，最少需要多少成本能使得状态 $x$ 收敛至 $x_F$，公式如下
$$
\begin{equation} Q^j(x) = \left\lbrace \begin{array}{ll}\min \limits { (i,t) \in F^j(x)} J^i(x), & \text{if } x \in \mathcal {SS}^j \ +\infty, & \text{if } x \notin \mathcal {SS}^j \end{array}\right. \end{equation}
$$
LMPC 公式的构建
$$
\begin{align} &J_{t\rightarrow t+N}{{\text{LMPC}},j}(x_tj)=\min\limits_{u_{t|t},\ldots,u_{t+N-1|t}} \bigg [ \sum {k=t}^{t+N-1} h(x,u_{k|t}) \nonumber + Q^{j-1}(x_{t+N|t}) \bigg]\ &\text{s.t. }\nonumber \ & x_{k+1|t}=f(x_{k|t},u_{k|t})\quad \ \forall k \in [t, \ldots, t+N-1]\ & x_{k|t} \in \mathcal {X}, \ u_{k|t} \in \mathcal {U}\quad \ \forall k \in [t, \ldots, t+N-1]\ &x_{t+N|t} \in \ \mathcal {SS}^{j-1}\ &x_{t|t}=x_t^j \end{align}
$$
我们在每一次预测时，将终端状态 $x_{t+N}^j$ 都限制在 $\mathcal {SS}$ 集中，因此终端成本 $Q^j(x_{t+N})$ 为历史数据中从终端状态 $x_{t+N}^j$ 收敛至 $x_{F}$ 所需的最低成本.
本 LMPC 算法正是通过这两个部件来保证迭代可行性、渐进稳定性、迭代成本的非增性，以及当系统收敛至一固定轨迹时，保证该轨迹的最优性，接下来证明这四个性质。
三、证明
迭代可行性
我们使用数学归纳法来证明 LMPC 算法在任何时候都是有解的：
假设 1：$SS^0$ 非空。可以通过 Standard MPC 或者其他算法获取一条可行轨迹 ${{\mathbf x}}^{0}$ ，初始化 $\mathcal {SS}$ 集
-
t = 0 时有解：
因为 $SS^0$ 非空，所以对任意一次迭代 $j, t=0$ 时都至少有一个解：即初始化轨迹 $\mathbf u^0, \ \mathbf x^0$ -
假设 $t$ 时刻有解，其解为：
$$
\begin{equation} [u_{t|t}{*,j}, u_{t+1|t}{,j},\ u_{t+2|t}{*,j}, \ldots, u_{t+N-1|t}{,j},\ u_{t*}{i*}, u_{t+1}{i},\ \ldots] \end{equation}
$$$$
\begin{equation} [x_{t|t}{*,j}, x_{t+1|t}{,j},\ x_{t+2|t}{*,j}, \ldots, x_{t+N-1|t}{,j},\ x_{t+N|t}{*,j}=x_{t}{i},\ x_{t*+1},\ \ldots] \end{equation}
$$其中，解的后半段 $[u_{t*}{i*}, u_{t+1}{i},\ \ldots]$ $[x_{t+N|t}{*,j}=x_{t}{i},\ x_{t*+1},\ \ldots]$ 为历史数据($\mathcal {SS^{j-1}}$)中使得终端成本最小的控制序列
$$
\begin{equation} Q{j-1}(x_{t+N|t}{,j}) = J{i}{t^*\rightarrow \infty }(x^{,j}) = \sum \limits _{k=t*} h(x_k{i},u_k{i*}). \end{equation}
$$ -
证明时刻 $t+1$ 时有解：
因为，我们考虑的是定常系统，可得 $x_{t+1}^{j} = x_{t+1|t}^{*,j}$，所以 $t+1$ 时刻，该问题最少有一个可行解$$
\begin{equation} [u_{t+1|t}{*,j}, u_{t+2|t}{,j},\ \ldots,\ u_{t+N-1|t}{*,j}, u_{t}{i},\ u_{t+1*}{i^},\ \ldots] \end{equation}
$$$$
\begin{equation} [x_{t+1|t}{*,j}, x_{t+2|t}{,j},\ \ldots,\ x_{t+N-1|t}{*,j}, x_{t+N|t}{,j}=x_{t*}{i*}, x_{t+1}{i},\ \ldots] \end{equation}
$$
渐近稳定性
证明：LMPC 算法会渐近收敛到平衡点 $x_F$
我们将 $J_{0\rightarrow N}{{\text{LMPC}},j}(x_tj)$ 定义为李雅普诺夫函数，其正定且仅在平衡点 $x_F$ 为 0，因此只需证明其函数值沿着闭环轨迹递减即可。
$$
\begin{align} J_{0\rightarrow N}{{\text{LMPC}},j}(x_tj)&=\min {u,\ldots,u_{t+N-1|t}} \bigg [ \sum {k=0}^{N-1} h(x,u_{k|t}) \nonumber + Q^{j-1}(x_{N|t}) \bigg ] \nonumber \ &= h(x_{t|t}{*,j},u_{t|t}{,j}) + \sum _{k=1}^{N-1} h(x{*,j}_{t+k|t},u{,j}{t+k|t}) + Q{j-1}(x) \nonumber \ &= h(x_{t|t}{*,j},u_{t|t}{,j}) + \sum _{k=1}^{N-1} h(x{*,j}_{t+k|t},u{,j}{t+k|t}) + \sum {k=t*} h(x{i*},u{i*}) \nonumber \ &= h(x_{t|t}{*,j},u_{t|t}{,j}) + \sum _{k=1}^{N-1} h(x{*,j}_{t+k|t},u{,j}{t+k|t}) + h(x{i*}{t*},u{i*}_{t}) \ \nonumber + Q{j-1}(x{i*}_{t+1}) \nonumber \ &\geq h(x_{t|t}{*,j},u_{t|t}{,j}) + J_{0\rightarrow N}{{\text{LMPC}},j}(x_{t+1|t}{,j}) \end{align}
$$
比较 $A = J_{0\rightarrow N}{{\text{LMPC}},j}(x_tj)$，$B = h(x_{t|t}{*,j},u_{t|t}{,j}) + J_{0\rightarrow N}{{\text{LMPC}},j}(x_{t+1|t}{,j})$ 两个优化函数， $B$ 比 $A$ 多了一个可优化的变量，因此当且仅当 $B$ 的控制变量全与 $A$ 相同时 $B$ 取其最大值。所以，我们有以下不等式：
$$
\begin{align} J_{0\rightarrow N}{{\text{LMPC}},j}(x_{t+1}j)- &J_{0\rightarrow N}{{\text{LMPC}},j}(x_{t}j) \leq - h(x_{t}{j},u_{t}) < 0, \nonumber \ &\forall \ x_t^j \in R^n \setminus \lbrace x_F\rbrace \ \forall \ u_t^j \in R^m \setminus \lbrace 0\rbrace. \end{align}
$$
得证。
成本非增性
- 证明迭代成本不会随着迭代次数增加，即 $J_{0\rightarrow \infty }^{j-1}(x_S) \geq J_{0\rightarrow \infty }^{j}(x_S) \quad \forall k\geq 0$
$$
\begin{align} J_{0\rightarrow \infty }^{j-1}(x_S) &= \sum \limits _{t=0}^{\infty } h(x_t{j-1},u_t) \nonumber \ &=\sum \limits _{t=0}^{N-1} h(x_t{j-1},u_t) + \sum \limits _{t=N}^{\infty } h(x_t{j-1},u_t) \nonumber \ &\geq \sum \limits _{t=0}^{N-1} h(x_t{j-1},u_t) + Q{j-1}(x_N) \nonumber \ &\geq \min {u_0,\ldots,u{N-1}} \left[ \sum {k=0}^{N-1} h(x_k,u_k) + Q^{j-1}(x_N) \right] \nonumber \ &= J{{\text{LMPC}},j}(x_0). \end{align}
$$
最优性
- 证明假如 x 和 u 在第无穷次迭代的时候收敛为一确定轨迹，那么其为该轨迹为该问题的最优轨迹
前提：
- system(1)、控制器(23)(25)
- 假设 1：SS0 非空；假设 2：问题(5)严格凸
- 第无穷次的 x 和 u 都收敛 -> 第无穷次的 SS 集和 Q 也收敛
- 第无穷次 k 时刻的状态属于 k+1 时刻的承上集，k+1 时刻的状态属于 k 时刻的启下集
重新定义一个新的最优化问题（41），与原 LMPC 最优化问题相比，（41）的预测步长为 $T$，且（41）的终端集时原 LMPC 最优化问题的子集，如果 LMPC 最优化问题的解对于（41）也是可行的，那么该解也是 LMPC 最优化问题最优的解
$$
\begin{align} &\tilde{J}{t\rightarrow t+T}^*(x_t)=\min {u,\ldots,u{T-1}} \sum {k=0}^{T-1} h(x,u_{k})+Q^{\infty }(x_T)\tag{41a}\ &\text{s.t. }\nonumber \ &\quad x_{k+1}=f(x_{k},u_{k})\ \forall k \in [0, \ldots, T-1]\tag{41b}\ &\quad x_{k} \in \mathcal {X}, \ u_{k} \in \mathcal {U}\ \forall k \in [0, \ldots, T-1]\tag{41c}\ &\quad x_{0}=x_t,\ x_{T} = x_{t+T}^{\infty }\tag{41d} \end{align}
$$
先证明 $({\mathbf x}^{\infty }{t:t+N}, {\mathbf u}^{\infty })$ 是当 $T = N$ 时问题（41）的最优解：
$$
\begin{align} &{{\mathbf x}}^{\infty }{t:t+N} = [x^{\infty },\ x_{t+1}^{\infty },\ \ldots,\ x_{t+N}^{\infty }] \nonumber \ &{{\mathbf u}}^{\infty }{t:t+N} = [u^{\infty },\ u_{t+1}^{\infty },\ \ldots,\ u_{t+N-1}^{\infty }] \end{align}
$$
$$
\begin{align} J_{0\rightarrow N}^{{\text{LMPC}},\infty } & (x_{t}^{\infty }) \geq h(x_{t}^{\infty },u_{t}^{\infty }) + J_{0\rightarrow N}^{{\text{LMPC}},\infty }(x_{t+1}^{\infty }) \phantom{\Big ]} \nonumber \ \phantom{\Big ]} &\geq h(x_{t}^{\infty },u_{t}^{\infty }) + h(x_{t+1}^{\infty },u_{t+1}^{\infty }) + J_{0\rightarrow N}^{{\text{LMPC}},\infty }(x_{t+2}^{\infty }) \nonumber \ &\geq \left[ \sum {k=0}^{T-1} h(x^{\infty },u_{t+k}^{\infty }) + J_{0\rightarrow N}^{{\text{LMPC}},\infty }(x_{t+T}^{\infty }) \right]\ \forall \ T>0.\tag{42} \end{align}
$$
$$
\begin{equation} J_{0\rightarrow N}^{{\text{LMPC}},\infty }(x_{t+T}^{\infty })= Q^{\infty }(x_{t+T}^{\infty }).\tag{43} \end{equation}
$$
$$
\begin{equation} J_{0\rightarrow N}^{{\text{LMPC}},\infty }(x_{t}^{\infty }) \geq \sum {k=0}^{N-1} h(x^{\infty },u_{t+k}^{\infty }) + Q^{\infty }(x_{t+N}^{\infty }).\tag{44} \end{equation}
$$
再证明 $({\mathbf x}^{\infty }{t:t+N+1}, {\mathbf u}^{\infty })$ 是当 $T = N+1$ 时问题（41）的最优解：
所以，可以以此类推 $T = N+2，T = N+3 …$ 的情形，得证 $({\mathbf x}^{\infty }, {\mathbf u}^{\infty })$ 是问题（41）无限域时的最优解
四、实例
实例如下， CLQR 问题的控制目标为 $J^*$：
$$
\begin{align} J_{0\rightarrow \infty }^*(x_S)&=\min _{u_0, u_1,\ldots } \sum \limits _{k=0}^{\infty } \Big [ ||x_k||_2^2 + ||u_k||2^2 \Big ]\ \text {s.t. } &x= \left[\begin{array}{ll}1 & 1 \ 0 & 1 \end{array}\right] x_k + \left[\begin{array}{ll}0 \ 1 \end{array}\right] u_k\quad \ \forall k\geq 0\ &x_0=[-3.95 \ -0.05]^T \end{align}
$$
$$
\begin{align} & \left[\begin{array}{ll}-4 \ -4 \end{array}\right] \leq x_k \leq \left[\begin{array}{ll}4 \ 4 \end{array}\right] \ \forall k\geq 0\ &-1 \leq u_k \leq 1 \forall k\geq 0. \end{align}
$$