推导仿人机器人机器人的动力学模型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}\)。
图1-MPC控制器流程参考文献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.