仿人机器人运动控制（2）：模型预测控制（MPC）
推导仿人机器人机器人的动力学模型MPC控制公式与应用等内容，并给出了相应的公式和参考文献。
MPC 基本概念
模型预测控制（Model Predictive Control, MPC）是一种滚动优化控制方法，其核心思想是：利用当前时刻系统的状态合约束条件，对未来一段时间（预测区间）的状态、输入变量进行预测，并求解出一组最优控制序列；随后只选取该组序列中的第一个结果，应用于系统。重复该操作，直至系统达到期望状态。
可以理解为下象棋，脑海中预测评估未来N步棋的策略后，下出第一步棋
考虑如下离散型状态空间方程
\[ \boldsymbol{x}_{k+1} = f(\boldsymbol{x}_{k}, \boldsymbol{u}_k) \tag{1} \]
系统的性能代价指标\(J\)为
\[ J = h(\boldsymbol{x}_N, \boldsymbol{x}_d) + \sum_{k=1}^{N_p-1}g(\boldsymbol{x}_k, \boldsymbol{x}_d, \boldsymbol{u}_k) \tag{2} \]
其中
- 预测区间长度定义为\(N_p\)，控制区间长度定义为\(N_c\)
- 系统在\(k\)时刻的初始状态为\(\boldsymbol{x}_k\)
- 系统在\(d\)时刻的目标状态为\(\boldsymbol{x}_d\)
- \(h()\)项表示状态差异指标
- \(g()\)项表示控制代价指标
MPC滚动优化控制会求解预测区间\(N_p\)内的最优化问题（代价\(J\)最小），计算出最优控制序列\(\boldsymbol{u}_{k,k}, \boldsymbol{u}_{k+1,k},...,\boldsymbol{u}_{k+N_c-1,k}\)。
然后根据系统状态方程（1），同一侧系统在这样控制序列下的状态值变化\(\boldsymbol{x}_{k,k}, \boldsymbol{x}_{k+1,k},...,\boldsymbol{x}_{k+N_p-1,k}\)。
完成输入控制量的计算后，只对系统施加\(\boldsymbol{u}_{k,k}\)，丢弃其它控制序列，以此循环往复。
通过二次规划求解 MPC 问题
MPC 问题一般转化为二次规划问题（Quadratic Programming, QP）,QP的标准形式表示为
\[ \begin{array}{ll} \underset{x}{\operatorname{min}} & \frac{1}{2} x^T P x+q^T x \\ \text { s.t. } & G x \leq h \\ & A x=b \\ & l b \leq x \leq u b \end{array} \tag{3} \]
即满足约束条件下，求解出令代价指标\(J\)最小的\(\boldsymbol{x}\)值。
参考 qpsolvers/quadratic-programming，Python 举例如下
from numpy import array, dot
from qpsolvers import solve_qp
M = array([[1., 2., 0.], [-8., 3., 2.], [0., 1., 1.]])
P = dot(M.T, M) # quick way to build a symmetric matrix
q = dot(array([3., 2., 3.]), M).reshape((3,))
G = array([[1., 2., 1.], [2., 0., 1.], [-1., 2., -1.]])
h = array([3., 2., -2.]).reshape((3,))
A = array([1., 1., 1.])
b = array([1.])
x = solve_qp(P, q, G, h, A, b, solver="osqp")
print(f"QP solution: x = {x}")
关于如何 MPC 推导为 QP 形式，强烈推荐 DR_CAN 老师的讲解，下文只记录推导后的结论。
模型预测控制器求解流程
考虑线性时不变系统离散形式的状态空间方程一般形式
\[ \boldsymbol{x}_{k+1} = \boldsymbol{A}\boldsymbol{x}_k + \boldsymbol{B}\boldsymbol{u}_k \tag{4} \]
其代价函数 \(J\)
\[ J=\frac{1}{2} \boldsymbol{x}_{\left[k+N_{\mathrm{p}} \mid k\right]}^{\mathrm{T}} \boldsymbol{S} \boldsymbol{x}_{\left[k+N_{\mathrm{p}} \mid k\right]}+\frac{1}{2} \sum_{i=0}^{N_{\mathrm{p}}-1}\left[\boldsymbol{x}_{[k+i \mid k]}^{\mathrm{T}} \boldsymbol{Q} \boldsymbol{x}_{[k+i \mid k]}+\boldsymbol{u}_{[k+i \mid k]}^{\mathrm{T}} \boldsymbol{R} \boldsymbol{u}_{[k+i \mid k]}\right] \tag{5} \]
考虑系统线性约束条件
\[ \begin{gathered} \mathcal{M}_{[k+i]_{m \times n}} x_{[k+i]_{n \times 1}}+\mathcal{F}_{[k+i]_{m \times p}} u_{[k+i]_{p \times 1}} \leqslant \beta_{[k+i]_{m \times 1}}, \quad i=0,1,2, \cdots, N_{\mathrm{p}}-1 \\ \mathcal{M}_{N \mathrm{p}_{l \times n}} \boldsymbol{x}_{\left[k+N_{\mathrm{p}}\right]_{n \times 1}} \leqslant \boldsymbol{\beta}_{N \mathrm{p}_{l \times 1}} \end{gathered} \tag{6} \]
其中各项变量解释如下：
- \(\boldsymbol{A}\)为状态矩阵，\(\boldsymbol{B}\)为控制矩阵，与系统本身相关，需要靠参数辨识获取；
- \(\boldsymbol{S}=\left[\begin{array}{ccc}s_1 & \cdots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \cdots & s_n\end{array}\right], \quad s_1, s_2, \cdots, s_n \geqslant 0\)，表示系统末端代价；
- \(Q=\left[\begin{array}{ccc}q_1 & \cdots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \cdots & q_n\end{array}\right], \quad q_1, q_2, \cdots, q_n \geqslant 0\)，表示系统运行代价；
- \(\boldsymbol{R}=\left[\begin{array}{ccc}r_1 & \cdots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \cdots & r_p\end{array}\right], \quad r_1, r_2, \cdots, r_p>0\)，标识系统控制量代价；
- 公式（6）表示通用的约束条件，约束每个时刻输入控制量与状态变量的组合；
如图 1 所示，MPC 控制分为 2 个阶段：（1）离线操作：计算系列中间矩阵，转换为QP问题；（2）在线操作：在线实时求解 QP 问题，并执行控制 \(\boldsymbol{u}\)。
参考文献
- Di Carlo, Jared, et al. “Dynamic locomotion in the mit cheetah 3 through convex model-predictive control.” 2018 IEEE/RSJ international conference on intelligent robots and systems (IROS). IEEE, 2018.