面向未知负载的人形机器人自适应全身MPC与最优姿态规划

Adaptive Whole-Body MPC with Optimal Posture Planning for Humanoid Robots under Unknown Payloads

作者：Manh Cuong Nguyen, Duc Cuong Vu, Viet Anh Vu, Duc Tuan Pham, Toan Nguyen, Minh Hiep Phung, Ngo Anh Vien, Minh Nhat Vu · arXiv：2601.12972 · 论文链接 · 官方代码：未公开 · 关联依赖：OCS2

图1：任务从蹲下与抓取开始，经过负载保持和提升，再到行走、下蹲与放置。模糊补偿贯穿所有阶段，用来约束质心偏移。

一句话总结

这篇论文没有把未知负载单独当作可辨识参数，而是让质心MPC负责约束下的名义运动，让基于Capture Point误差的模糊补偿处理负载引起的模型失配，再让三重积分姿态规划生成蹲起与搬运轨迹；三者组成一条可上真实人形平台的闭环。

研究背景与动机

人形机器人进入人工环境的价值，来自它能同时完成移动、操作与感知。工业搬运正是这类任务最直接的交点：机器人要走到物体附近，蹲下抓取，站起来承载，再带着物体行走或放置。这个序列每一步都改变全身动力学，但工业部署中的物体质量、形状和惯量常常不能事先精确给出。

负载不确定性的难点不只是“多了一个重量”。抓取瞬间，机器人质量矩阵、质心位置和惯性张量同时改变；支撑腿与物体形成的接触图也会变化。若控制器只按名义模型预测，质心投影可能很快离开支撑多边形，踝关节或膝关节力矩也容易饱和。此时任何只靠反馈放大的方法都会遇到同一问题：误差出现时，系统已经接近失稳。

姿态过渡进一步放大了模型失配。蹲下和站起不是简单的轨迹插值，髋、膝、踝三个关节必须保持运动学和动力学一致；骨盆的俯仰角一旦变化，踝关节和髋关节都要重新分配力矩。如果轨迹在低层才暴露不可行，高频控制器只能拼命追赶一个物理上不该执行的参考。

传统全身控制器在名义运动上很强，但多数假设负载已知；模糊控制擅长在模型不精确时把可测误差映射成有界补偿，却通常缺少约束处理和优化。论文把两者的分工划得很清楚：MPC保留约束、接触与优化的结构，模糊项只做残差补偿，不替代预测层，也不去在线辨识负载。

作者的现实目标是可上硬件的搬运闭环，而不是离线演示。因此整个框架必须分层运行：慢层做非线性MPC，中层做力矩补偿和分配，低层在1 kHz以上执行力矩。这种频率划分决定了方法不能把所有问题塞进一个大优化，而要把约束、稳定性和平滑性分别交给合适的时间尺度。

预备知识：从全身动力学到质心模型

人形机器人可以写成浮动基座加驱动关节的形式。设广义坐标为

$$\mathbf{q}=[\mathbf{q}_{b}^{\top},\mathbf{q}_{j}^{\top}]^{\top},\qquad \mathbf{q}_{b}\in SE(3),\quad \mathbf{q}_{j}\in\mathbb{R}^{n_j}$$

其中 $\mathbf{q}_b$ 是浮动基位姿，$\mathbf{q}_j$ 是 $n_j$ 个驱动关节。完整动力学既要描述质量惯量，也要描述接触约束。论文采用带接触的Euler-Lagrange形式：

$$\mathbf{H}_{bb}(\mathbf{q})\ddot{\mathbf{q}}_{b}+\mathbf{H}_{bj}(\mathbf{q})\ddot{\mathbf{q}}_{j}+\mathbf{n}_{b}(\mathbf{q},\dot{\mathbf{q}})=\mathbf{J}_{cb}^{\top}(\mathbf{q})\boldsymbol{\lambda}$$

$$\mathbf{H}_{jb}(\mathbf{q})\ddot{\mathbf{q}}_{b}+\mathbf{H}_{jj}(\mathbf{q})\ddot{\mathbf{q}}_{j}+\mathbf{n}_{j}(\mathbf{q},\dot{\mathbf{q}})=\boldsymbol{\tau}_{j}+\mathbf{J}_{cj}^{\top}(\mathbf{q})\boldsymbol{\lambda}$$

$$\mathbf{J}_{i}(\mathbf{q})\ddot{\mathbf{q}}+\dot{\mathbf{J}}_{i}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}=0,\qquad \forall i\in\mathcal{C}$$

这里 $\mathbf{H}_{bb},\mathbf{H}_{bj},\mathbf{H}_{jb},\mathbf{H}_{jj}$ 是质量矩阵分块，$\mathbf{n}_b,\mathbf{n}_j$ 合并重力、科氏力和离心力，$\boldsymbol{\tau}_j$ 是关节力矩，$\mathcal{C}$ 是活动接触集合。第三式保证接触点不穿透、不打滑：对每个接触，其Jacobian下的加速度为零。

完整动力学实时优化代价高，所以预测层降到质心动力学。把质心动量写成 $\mathbf{h}=[\mathbf{h}_l^{\top},\mathbf{h}_a^{\top}]^{\top}$，线性与角动量满足

$$\dot{\mathbf{h}}=\begin{bmatrix}\sum_{i\in\mathcal{C}}\mathbf{F}_{i}+m\mathbf{g}\\ \sum_{i\in\mathcal{C}}\left(\mathbf{r}_{i}\times\mathbf{F}_{i}+\mathbf{M}_{i}\right)\end{bmatrix}$$

$\mathbf{F}_i,\mathbf{M}_i$ 是接触力和力矩，$\mathbf{r}_i$ 是接触点相对质心的位置，$m$ 是当前总质量，$\mathbf{g}$ 是重力向量。质心动量又通过质心动量矩阵 $\mathbf{A}(\mathbf{q})$ 与广义速度相关：$\mathbf{h}=\mathbf{A}(\mathbf{q})\dot{\mathbf{q}}$。于是取 $\mathbf{x}=[\mathbf{h}^{\top},\mathbf{q}_b^{\top},\mathbf{q}_j^{\top}]^{\top}$，系统可压缩成

$$\dot{\mathbf{x}}=\mathbf{f}(\mathbf{x},\mathbf{u})$$

这个降阶不是为了丢掉约束，而是把高频关节细节交给下层，让50 Hz的优化层有足够时间处理摩擦锥、支撑多边形和运动极限。

方法详解

系统的输入端分成两条：手动模式下操作杆给出骨盆参考速度；姿态规划模式下，优化器直接生成蹲下、起立或后仰的参考。两种模式最终都汇入质心MPC。MPC生成骨盆运动和接触力参考，全身控制器把这些参考转成关节力矩，模糊平衡控制器根据实时平衡误差叠加修正力矩，最后通过支撑腿分配到关节。这个结构的关键不是每个模块单独有多复杂，而是让预测、补偿和执行在错误传播前形成分工。

flowchart TB
    H[Desired pelvis height and pitch] --> P[Waypoint posture planner]
    P --> R[Smooth joint references]
    W[Joystick and walking mode] --> M[Centroidal MPC]
    R --> M
    M --> C[Contact forces and motion references]
    C --> B[Whole-body controller]
    S[State estimation] --> E[Capture point error]
    E --> F[Fuzzy balance controller]
    F --> A[Torque allocation]
    B --> A
    A --> J[Leg joint torque commands]
    J --> L[Humanoid low level control]
    L --> S
    P --> R2[Support polygon and joint limit checks]
    R2 --> P

图2：论文的闭环不是“MPC输出直接下发电机”，而是姿态规划、质心预测、模糊补偿与关节力矩分配形成四个反馈面。

非线性MPC生成约束内参考

在预测区间 $T$ 上，质心层求解

$$\min_{\mathbf{u}(\cdot)}\quad \Phi(\mathbf{x}(T))+\int_{0}^{T}l(\mathbf{x}(\rho),\mathbf{u}(\rho))\,d\rho$$

$$\text{s.t.}\quad \dot{\mathbf{x}}=\mathbf{f}(\mathbf{x},\mathbf{u}),\quad \mathbf{x}(0)=\mathbf{x}_{t},\quad (\mathbf{x},\mathbf{u})\in\mathcal{X}\times\mathcal{U}$$

$l$ 和 $\Phi$ 分别是阶段代价与终端代价，$\mathcal{X}$ 和 $\mathcal{U}$ 是状态与输入约束集。约束不是装饰：支撑脚的摩擦锥和压力中心必须在接触 wrench 集合内，摆动脚法向速度按阶段切换，关节位置和速度都要保持在极限内。这样，MPC给出的不是理论最优但无法执行的轨迹，而是在当前接触模式下物理可执行的参考。

下层采用逆动力学而不是再解一个任务空间QP。MPC解出的第一个输入是 $\mathbf{u}(0)=[\dot{\mathbf{q}}_j^{*\top},\boldsymbol{\lambda}^{*\top}]^{\top}$。为了让关节指数跟踪，取

$$\ddot{\mathbf{q}}_{j}^{*}=-K_{p}\mathbf{e}_{j}-K_{d}\dot{\mathbf{e}}_{j}$$

其中 $\mathbf{e}_j=\mathbf{q}_j-\mathbf{q}_j^*$，$\dot{\mathbf{e}}_j=\dot{\mathbf{q}}_j-\dot{\mathbf{q}}_j^*$。浮动基加速度由接触动力学反算：

$$\ddot{\mathbf{q}}_{b}^{*}=\mathbf{H}_{bb}^{-1}\left(\mathbf{J}_{cb}^{\top}\boldsymbol{\lambda}^{*}-\mathbf{H}_{bj}\ddot{\mathbf{q}}_{j}^{*}-\mathbf{n}_{b}\right)$$

代入关节动力学，得到力矩指令

$$\boldsymbol{\tau}_{j}=\mathbf{H}_{jb}\ddot{\mathbf{q}}_{b}^{*}+\mathbf{H}_{jj}\ddot{\mathbf{q}}_{j}^{*}+\mathbf{n}_{j}-\mathbf{J}_{cj}^{\top}\boldsymbol{\lambda}^{*}$$

这样做的理由是接触与动态可行性已在MPC层处理，下层若再解QP，只会引入二次优化和任务权重调试负担。

模糊补偿：不估计负载，只估计平衡误差

论文的模糊控制器输入不是负载质量，而是质心与支撑多边形的关系。在线性倒立摆假设下，Capture Point 为

$$\boldsymbol{\xi}=\mathbf{r}_{\text{com}}+\frac{\dot{\mathbf{r}}_{\text{com}}}{\omega},\qquad \omega=\sqrt{\frac{g}{z_c}}$$

$\mathbf{r}_{\text{com}}$ 是质心在地面上的投影，$z_c$ 是名义质心高度。支撑中心 $\mathbf{c}_{\text{supp}}$ 由站立脚接触点加权得到。平衡误差取

$$\mathbf{e}=\boldsymbol{\xi}-\mathbf{c}_{\text{supp}}=\mathbf{r}_{\text{com}}-\mathbf{c}_{\text{supp}}+\frac{\dot{\mathbf{r}}_{\text{com}}}{\omega}$$

这个量直接回答一个问题：如果机器人当前速度保持不变，它下一步会落在支撑多边形的哪里。前向分量 $e_x$ 由骨盆俯仰力矩修正，侧向分量 $e_y$ 由横滚力矩修正。误差积分 $\mathbf{e}_{\text{int}}=\int\mathbf{e}\,dt$ 带 anti-windup，满足 $\left|\mathbf{e}_{\text{int}}\right|\le e_{\text{int,max}}$，避免长时间失衡后补偿器一直积到饱和。

每个轴的模糊系统有四个层。第一层把偏差、质心速度和积分映射到 NB、NS、Z、PS、PB 五个语言值。对集合 $\ell$ 的隶属度是高斯函数：

$$\mu_{\ell}(x)=\exp\left(-\frac{(x-c_{\ell})^{2}}{2\sigma_{\ell}^{2}}\right)$$

第二层为每轴建立 $5\times5\times5=125$ 条规则，形式为“若偏差是 $\mathcal{A}_i$，速度是 $\mathcal{B}_j$，积分是 $\mathcal{C}_k$，则输出 $y_{ijk}$”。规则强度用乘积t-norm：

$$w_{ijk}=\mu_{\text{dev}}^{(i)}(e)\cdot\mu_{\text{vel}}^{(j)}(\dot{r})\cdot\mu_{\text{int}}^{(k)}(e_{\text{int}})$$

后件把基础PD式项和积分项相加：

$$y_{ijk}=\text{clip}_{[-1,1]}\left(y_{\text{base}}^{(ij)}+\Delta_{\text{int}}^{(k)}\right)$$

最后输出加权平均并按轴缩放、饱和：

$$\tau=S\cdot\frac{\sum_{i,j,k}w_{ijk}y_{ijk}}{\sum_{i,j,k}w_{ijk}},\qquad |\tau|\le\tau_{\max}$$

作者没有把模糊控制器包装成黑箱智能。它更像一个随偏差、速度和积分共同改变增益的饱和PID：偏差衰减时不过度纠正，长时间小偏差时补入积分作用，输出始终有硬件安全上限。模糊参数由专家调好，并在所有实验中保持不变。

图3：模糊补偿器的输入是偏差、速度与积分；输出是有限幅值的骨盆俯仰/横滚修正力矩。

修正力矩只进入支撑腿

得到任务空间补偿力矩 $\boldsymbol{\tau}_{\text{comp}}^{\text{task}}=[\tau_{\text{pitch}},\tau_{\text{roll}}]^{\top}$ 后，论文没有让上肢参与平衡。手臂正抓着物体，上肢摆动会扰动抓取，也改变惯性。因此分配问题只在支撑腿内求解：

$$\boldsymbol{\tau}_{\text{leg}}^{*}=\arg\min_{\boldsymbol{\tau}_{\text{leg}}}\ \boldsymbol{\tau}_{\text{leg}}^{\top}\boldsymbol{\tau}_{\text{leg}}$$

$$\text{s.t.}\quad \Omega(\boldsymbol{\tau}_{\text{leg}})=\boldsymbol{\tau}_{\text{comp}}^{\text{task}}$$

$\Omega$ 把腿部关节力矩映射为骨盆处可观察的任务空间力矩。最小二乘目标是选择能量最小、扰动最小的关节补偿组合。这个设计很务实：修正力矩不进入手臂，抓取不变；不进入摆动腿，步态参考也不被平衡误差污染。

姿态规划：骨盆位置不是任意选的

蹲下或站起需要同时决定骨盆高度、俯仰角和水平位置。如果只固定高度和俯仰，水平位置有无穷多解，但真正可行的解必须落在支撑多边形内，且满足关节限位。论文把腿部简化为三连杆平面模型，关节向量为

$$\mathbf{q}_{w}=[q_{h},q_{k},q_{a}]^{\top}$$

分别代表髋、膝、踝。给定期望骨盆高度 $h$、俯仰 $\theta$ 和候选水平位置 $p_x$，算法从支撑多边形内搜索：用逆运动学得到 $\mathbf{q}_{wr}$；若关节不在限位集合 $\mathcal{Q}$ 内，则按 $\operatorname{sign}(\theta)\varepsilon$ 移动 $p_x$，直到找到可行姿态。这个搜索避免了一个常见错误——让高频控制器去硬拉一个几何上不可达的骨盆参考。

逆运动学用余弦定理直接写出。设 $l_1,l_2$ 是大腿和小腿长度，$d_0$ 是髋到踝距离：

$$q_{k}=\pi-\arccos\left(\frac{l_{1}^{2}+l_{2}^{2}-d_{0}^{2}}{2l_{1}l_{2}}\right)$$

$$q_{h}=\frac{\pi}{2}-\arccos\left(\frac{l_{1}^{2}+d_{0}^{2}-l_{2}^{2}}{2l_{1}d_{0}}\right)-\arctan 2(h,p_{x})$$

$$q_{a}=-q_{h}-q_{k}-\theta$$

这个闭式解不追求全身所有自由度，而是提取蹲起中真正影响平衡的腿部几何。它的好处是每个候选骨盆位置都能快速判断可行性，让搜索可以被优化器反复调用。

图4：平面三连杆模型保留髋、膝、踝的主要几何关系，用于快速生成骨盆姿态候选。

三重积分把平滑性写进动力学

得到参考姿态后，论文没有直接对关节角做插值，而是用三重积分模型生成轨迹。状态包含位置、速度和加速度：

$$\boldsymbol{\eta}=[\mathbf{q}_{w}^{\top},\dot{\mathbf{q}}_{w}^{\top},\ddot{\mathbf{q}}_{w}^{\top}]^{\top},\qquad \boldsymbol{\omega}=\dddot{\mathbf{q}}_{w}$$

离散递推为

$$\boldsymbol{\eta}_{k+1}=\boldsymbol{\Phi}\boldsymbol{\eta}_{k}+\boldsymbol{\Gamma}_{1}\boldsymbol{\omega}_{k}+\boldsymbol{\Gamma}_{2}\boldsymbol{\omega}_{k+1}$$

其中 $\boldsymbol{\Phi}$、$\boldsymbol{\Gamma}_1$、$\boldsymbol{\Gamma}_2$ 由时间步 $\Delta t$ 和 $\mathbf{I}_3$ 的Kronecker积构成。以jerk为输入意味着连续性约束直接进入动力学，而不是事后用滤波器抹平。

轨迹优化写成有限时域二次型：

$$\min_{\boldsymbol{\eta},\boldsymbol{\omega}}\sum_{k=0}^{N}\left(\|\mathbf{q}_{w,k}-\mathbf{q}_{wr,k}\|_{\mathbf{W}_{x}}^{2}+\|\boldsymbol{\omega}_{k}\|_{\mathbf{W}_{u}}^{2}\right)$$

约束包括三重积分递推、初始关节状态、位置/速度/加速度/jerk边界，以及质心投影函数 $\gamma(\mathbf{q}_{w,k})$ 必须落在支撑多边形内：

$$\boldsymbol{\eta}_{k+1}=\boldsymbol{\Phi}\boldsymbol{\eta}_{k}+\boldsymbol{\Gamma}_{1}\boldsymbol{\omega}_{k}+\boldsymbol{\Gamma}_{2}\boldsymbol{\omega}_{k+1}$$

$$\mathbf{q}_{w0}=\mathbf{q}_{w,\text{init}},\qquad \boldsymbol{\eta}_{k}\in\mathcal{N},\quad \boldsymbol{\omega}_{k}\in\mathcal{W},\quad \gamma(\mathbf{q}_{w,k})\in\texttt{support\_polygon}$$

这样，蹲下时骨盆会主动前移，站起时重心也随几何变化；轨迹不是先把关节角排好再检查稳定，而是把质心稳定性作为硬约束。

图5：硬件实验中，机器人保持双腿支撑，手臂只负责抓取，平衡修正集中分配给腿部。

实验与实现

平台是28自由度人形机器人，每条手臂7个自由度，每条腿6个自由度。控制频率分层：质心MPC 50 Hz，自适应平衡控制器 500 Hz，低层力矩控制器高于1 kHz。仿真在MuJoCo中做模块级验证；硬件实验覆盖静态提举和“提起-行走-放置”两个场景。

图6：左侧标准质心MPC在重负载下前倾倒下；右侧自适应MPC主动降低姿态，把质心留在稳定裕度内。

方法	行走	物理约束	自适应平衡	姿态规划	典型应用
Reduced-Order MPC	支持	不支持	不支持	不支持	鲁棒行走
MPC + WBC	支持	支持	不支持	不支持	动态行走与全身控制
Matrix Exponential-MPC + WBPC	支持	支持	不支持	不支持	行走
Flexible MPC	支持	支持	不支持	不支持	变速行走
NMPC + Gait Adaptation	支持	支持	支持	不支持	姿态调节与推力恢复
Kinodynamic MPC + Gait-Net	支持	支持	支持	不支持	不同频率的复杂地形行走
Three Particle-MPC + WBC	不支持	支持	不支持	支持	连续精确全身蹲起
本文方法	支持	支持	支持	支持	未知负载下的行走操作

表1：论文中的MPC框架对比。本文的差异不是单一指标最高，而是把行走、约束、模糊平衡与姿态规划同时放进一个框架。

静态提举

硬件提举实验用7 kg和10 kg两个负载。7 kg场景中，约30 s机器人开始降低骨盆高度并前倾，蹲下过程约3 s；抓取物体后，55 s到80 s完成站起，最后把物体放回地面。骨盆高度、俯仰角和髋/膝/踝关节角在过程中没有出现明显的阶跃，说明三重积分规划把平滑性留在了轨迹本身。

10 kg场景更能看出补偿器的作用。20 s左右机器人进入弯腰姿态，30 s左右抓取；抓起后躯干动力学变化更大，骨盆高度和俯仰角出现小幅振荡，随后模糊控制器提高修正力矩，俯仰稳定后机器人继续恢复到名义直立姿态。这里没有单独报告负载质量估计，控制器响应的是Capture Point误差变化，而不是等待某个质量参数收敛。

实验	负载	关键时间点	观察结果
静态提举	7 kg	约30 s开始蹲下，蹲下约3 s，55–80 s站起	骨盆高度和俯仰平滑变化，随后完成放置
静态提举	10 kg	约20 s弯腰，约30 s抓取	初始提起出现小幅振荡，补偿稳定后恢复直立
提举与行走	7 kg	83 s抓取，110 s回到站姿，140–155 s行走	全程稳定完成提起、承载、行走和放置
仿真验证	未知重物	标准MPC失败阶段1–3	自适应方法主动蹲低并维持质心约束

表2：论文给出的实验时间线。它展示的是闭环行为顺序，而不是只挑一个峰值指标。

行走中的负载搬运

行走场景的负载是7 kg。流程是：83 s弯腰抓取，110 s提物回到home姿态，140–155 s承载行走15 s，最后停止、下蹲放置。论文特别说明，模糊参数没有为行走重新调参。这一点很重要：如果补偿器只为静态提举调好，却在行走中失效，那它就只是针对特定实验的补丁；同一组参数覆盖两个阶段，才说明它捕捉的是平衡误差这类共性信号。

图7：真实平台验证覆盖静态提举与行走承载，而不只是低自由度简化模型。

模块	核心信号	输出	关键约束	作用
质心MPC	骨盆参考、接触模式、质心状态	运动参考与接触wrench	摩擦锥、压力中心、关节限位、摆动脚速度	生成名义可行运动
逆动力学WBC	关节误差、接触wrench	关节力矩	指数跟踪、动力学一致性	不额外解任务空间QP
模糊平衡器	Capture Point误差、CoM速度、误差积分	俯仰/横滚补偿力矩	五值隶属度、125规则、输出饱和	补偿负载引起的失配
力矩分配	任务空间补偿力矩	支撑腿关节补偿	最小力矩范数、只作用于支撑腿	不扰动抓取与摆动腿
姿态规划	骨盆高度、俯仰、支撑多边形	蹲起关节轨迹	三重积分、jerk限位、CoM稳定	把平滑和可行性放进前馈

表3：五个模块的职责边界。每一层都只拿自己能可靠测量的信号。

局限与开放问题

第一，模糊规则和隶属度参数由专家手工设计。论文自己承认，参数通过经验调出，并且与PID的定量比较留待未来工作。这意味着方法在当前平台上有效，但换机器人或换任务时，可迁移性还没有被系统证明。

第二，Capture Point误差建立在线性倒立摆假设上。搬运重物时，躯干角动量、双臂负载惯性和多接触动力学都会影响真实失稳趋势；如果负载很大或形状复杂，二维俯仰/横滚补偿可能不足。

第三，力矩分配要求知道 $\Omega$，也就是支撑腿关节力矩到骨盆任务力矩的映射。真实机器人会有传动柔性、关节摩擦和估计噪声；论文没有讨论该映射的在线校正或误差传播。

第四，姿态规划以平面三连杆为代表。它能抓住蹲起的主要腿部几何，但真实搬运还会改变上肢姿态、躯干偏航和双脚接触。若物体遮挡足端或要求侧向进入，模型需要扩展。

第五，论文没有公开代码，也没有给出统一排行榜式指标。实验证据以成功行为、时间线和轨迹稳定性描述为主；与DOB、模型参考自适应或鲁棒控制的定量对照还没有完成。作者也把学习型补偿和主动扰动观测器列为未来方向。

结论

这篇论文的价值在于工程闭环：MPC处理已知约束下的名义最优，模糊补偿处理不可辨识负载留下的残差，姿态规划在抓取前就把骨盆放到可行位置。它没有声称模糊控制比现代学习控制更强，而是解释了为什么在负载突变、模型噪声和硬件安全约束同时存在时，一个有界、可解释、无需辨识的补偿器是务实选择。

对做机器人控制的人来说，可迁移的思想有三点：第一，不要让一个优化器同时承担所有不确定性；第二，补偿信号应当直接连接可测的失稳指标，而不是等待参数辨识；第三，平滑性要写成jerk或加速度动力学，不能靠低层滤波补救。若未来把规则参数改成学习得到，并加入主动扰动观测，这个框架会更有竞争力。

金句：与其先猜负载是什么，不如先问质心下一刻要去哪里；如果那里已经离开支撑多边形，任何名义最优都来不及救场。