企业官网建设流程全解析

简介：面向双足机器人行走控制与轨迹规划领域研究者和工程师的开源实现，聚焦ZMP（零力矩点）预览控制的离散控制行走模式生成，解决步行稳定性与轨迹跟踪问题。项目提供完整的Matlab与Python双语言源码：Matlab负责计算最优预览控制增益矩阵，Python则通过前馈控制完成系统仿真，并配有可直接执行的Jupyter Notebook，方便按步骤对照复现实验。压缩包共16个文件，涵盖6个M脚本（含增益配置、ZMP/CoM轨迹生成与预览控制计算）、1个py仿真脚本、1个ipynb笔记本、1个mat参数文件以及多张结果轨迹图，整体仅109KB，轻量且易于部署。源码变量均与相关论文符号一一对应，附带的README、LICENSE和x/y方向ZMP与CoM轨迹图片，能辅助读者深入理解控制机理。已有629人学习，适合具备一定控制理论基础、希望快速上手ZMP预览控制并开展二次开发的进阶学习者。 各位机器人方向的同行，今天聊一个绕不开的话题：双足人形机器人的稳定行走模式生成。标题里的这套东西——ZMP预览控制行走模式生成（ZMP-Preview-Control-WPG），核心就是用ZMP这个判据加预览控制算法，在Matlab里离线生成一条既满足动力学约束、又让机器人走得稳的重心轨迹。它不是某个实验室的封闭方案，而是双足控制领域公认的基础做法，当年很多经典研究都建立在这个框架上。对于正在做双足仿真、或者想搞懂人形机器人怎么把"走路的想法"变成"关节能执行的轨迹"的同学，把这份代码吃透，收益远不止是跑通一个Demo。

很多刚入门的同学会把行走模式生成和步态规划混为一谈，其实两者差别很大。简单说，步态规划解决的是"脚往哪迈、迈多宽、什么时候迈"，而行走模式生成解决的是"重心（CoM）怎么动，才能让脚底的压力中心（ZMP）始终落在支撑多边形内"。ZMP预览控制WPG正是后者中的一种经典实现：它基于线性倒立摆模型（LIPM），把重心运动与ZMP之间的关系描述成一个离散线性系统，然后利用未来若干个步态的参考ZMP作为"预览输入"，通过最优控制的方法在线或离线生成平滑、稳定的CoM轨迹。下面我从原理、数学化、代码架构到实际调试，把这套东西完整拆开讲一遍。

1. 为什么走路建模这么难，以及预览控制解决了什么

双足行走的困难根源在于这是一个欠驱动、非线性的高维度动力学问题。人形机器人全身可能有几十个自由度，如果直接对全身动力学做轨迹优化，计算量巨大，而且实时性根本没法保证。实际的工程做法和学术界的主流思路都是分而治之：先把"走路"解耦成"上半身姿态稳定"和"下半身支撑与摆动"两个问题，再用一个低维模型去描述最关键的重心稳定性。

这里引入的就是线性倒立摆模型（LIPM）。LIPM的思想非常简单粗暴：把整个机器人等效成一个质量集中在质心（CoM）的倒立摆，摆的支点就是ZMP。当CoM高度恒定、角动量变化可忽略时，CoM的水平运动与ZMP之间呈现一个线性关系：

[ \ddot{x} = \frac{g}{z_c}(x - p_x) ]

其中 ( z_c ) 是CoM高度，( p_x ) 是ZMP的x坐标，g是重力加速度。这本质上是一个一阶双积分器系统加反馈，但就是这个简单模型，抓住了行走稳定的核心矛盾：要让ZMP落在支撑脚与地面的接触多边形内，否则机器人就会绕脚掌边缘翻转摔倒。

但仅仅有LIPM还不够。经典的两步走方法是：先规划ZMP轨迹，再反解重心轨迹。问题是，如果直接用当前的ZMP倒推CoM，会得到发散的CoM解。原因在于LIPM本身是一个不稳定系统——倒立摆天生就是"往哪边倒"的，系统极点落在实轴两侧，其中一个在右半平面。这时预览控制（Preview Control）的价值就体现出来了：它和普通反馈控制最大的区别在于，控制器能够"看到"未来一段时间的参考输入，从而提前调整当前的控制量，让系统在不违反约束的前提下平滑地跟踪未来的目标。

我在实际仿真里有过很直观的体验：只依赖当前时刻的ZMP误差做反馈控制，重心的轨迹是抖的，脚落地时冲击力特别大；把未来1.6秒的ZMP参考轨迹引入预览控制项之后，整个人直立行走的过渡变得非常顺滑，重心轨迹呈现一种"蓄力—释放—制动"的自然节奏。这正是预览控制名字的来历：不是亡羊补牢，而是预判未来、提前响应。

2. 离散控制数学化：从LIPM到ZMP预览控制器的完整推导路径

数学化是这套代码的筋骨。你要是只想把代码跑起来，那可以直接跳过推导、看下一节；但如果你想改步态参数、或者把这个WPG移植到自己的机器人上，这部分躲不掉。我尽量用最直观的方式把推导链路理清楚，符号体系直接对齐常见的Matlab实现版本。

2.1 状态空间形式：把微分方程变成离散系统

先把LIPM方程改写成状态空间。取状态变量为CoM的位置 ( x ) 和速度 ( \dot{x} )，输入是CoM加速度 ( u = \ddot{x} )，输出是ZMP位置 ( p = x - \frac{z_c}{g}u )。连续时间状态空间为：

[ \dot{\mathbf{x}} = \begin{bmatrix} 0 & 1 \ 0 & 0 \end{bmatrix} \mathbf{x} + \begin{bmatrix} 0 \ 1 \end{bmatrix} u,\quad p = \begin{bmatrix} 1 & 0 \end{bmatrix} \mathbf{x} - \frac{z_c}{g} u ]

注意，输出方程里直接含有一个输入项 ( u )，这在控制理论里叫直接传递项。由于伺服控制、数值积分都需要离散化代码，我们通常以固定控制周期 ( T )（比如5ms）做零阶保持离散化。离散后的系统写成：

[ \mathbf{x}[k+1] = A \mathbf{x}[k] + B u[k],\quad p[k] = C \mathbf{x}[k] + D u[k] ]

其中：

[ A = \begin{bmatrix} 1 & T \ 0 & 1 \end{bmatrix},\quad B = \begin{bmatrix} \frac{T^2}{2} \ T \end{bmatrix},\quad C = \begin{bmatrix} 1 & 0 \end{bmatrix},\quad D = -\frac{z_c}{g} ]

这里有个关键细节：离散化时，如果控制周期取得太粗（比如50ms），零阶保持的相位滞后会让人感觉ZMP轨迹和CoM轨迹对不上，预览控制的优越性会被掩盖大半。我一般先用5ms、10ms分别跑一遍，对比ZMP跟踪误差，差距非常明显。

2.2 增广系统设计：把ZMP误差变成优化目标

预览控制的标准套路是构造增广系统。把ZMP参考值 ( p^{ref} ) 与系统输出之差做累加，得到新的状态 ( w[k+1] = w[k] + (p^{ref}[k] - p[k]) )，与原始状态一起组成增广状态：

[ \tilde{\mathbf{x}}[k+1] = \tilde{A} \tilde{\mathbf{x}}[k] + \tilde{B} u[k] + \tilde{G} p^{ref}[k] ]

性能指标取二次型：

[ J = \sum_{j=k}^{\infty} \left[ Q_e w[j]^2 + Q_x \mathbf{x}[j]^T \mathbf{x}[j] + R u[j]^2 \right] ]

其中 ( Q_e ) 是ZMP误差积分权重，( Q_x ) 是CoM状态权重，( R ) 是加速度输入权重。从数学上看，预览控制本质上就是模型预测控制（MPC）的一种特殊形态——只是预测时域取到无穷远、且参考是"未来已知"的。这个优化问题的解可以写成三项：一项是对当前状态的反馈项，一项是误差积分项，还有一项就是对未来参考输入的预览项。Matlab代码里通常用离散Riccati方程求解反馈增益，然后用一个离线算好的预览增益数组对未来的参考轨迹做卷积求和。

2.3 预览时域长度怎么定

预览时域 ( N_L ) 的长度非常讲究。如果太短，控制器"预判"不足，起不到平滑ZMP跟踪的作用；如果太长，一方面计算量增加，另一方面远期参考对当前决策的影响已经衰减到可以忽略。我的经验是：预览窗口覆盖一个完整步态周期的1.5到2倍。假设步态周期1.2秒、控制周期5ms，那预览步数在360到480之间，对应的预览时间约为1.8到2.4秒。你可以看预览增益数组中衰减到接近零的点，那个位置基本就是有效预览长度。

3. Matlab WPG代码的架构拆分：输入、核心计算与输出

这套代码的工程结构，我拆解下来大致是"参数配置—参考ZMP生成—控制器求解—轨迹输出"四大块。理解了每一块，你才算真正拥有这份代码，而不是仅仅让它run起来。

3.1 顶层脚本与参数配置

代码入口通常是一个参数配置文件或直接写在脚本头部。核心参数包括：

注意CoM高度的取值：它不是几何上的髋关节高度，而是整个机器人等效质心在站立时的实际高度。很多新手直接量了骨盆高度填进去，导致生成的轨迹看着合理、上真机就晃，多半是这个原因。

3.2 参考ZMP轨迹生成

这个环节决定了机器人走起来"像不像样"。一个完整的步态周期包含双支撑相和单支撑相，ZMP参考轨迹在双支撑相从后脚平稳过渡到前脚，在单支撑相则保持在前脚脚掌的中心附近。常用做法是从支撑脚踝关节坐标系下定义ZMP参考点，再通过步态时序拼接成全局坐标下的折线或平滑曲线。

一个常见改进是给ZMP参考轨迹加圆弧过渡或S曲线，避免突变。预览控制本身能容忍一定程度的参考突变，甚至这正是它的优势——但对真机来说，输入越平滑，关节力矩越平缓，机械冲击越小。所以我在这个阶段会做一个可选的三次样条平滑，但在最终输出的ZMP参考中会保留与步态时序严格对齐的拐点，以免模糊了单双支撑切换的物理边界。

3.3 控制器核心计算

控制器求解部分大致有三步：

第一步，求解离散Riccati方程，得到反馈增益和误差积分增益。Matlab里可以直接调用dare函数，或者自己迭代Riccati方程收敛。注意要注意规范化的数值规模：如果 ( Q_e ) 和 ( Q_x ) 的数量级差太多，Riccati方程求解容易出现数值病态。

第二步，离线计算预览增益。这个可以通过反向迭代求解，得到一组随预测时刻变化的系数向量，其实就是一个"未来参考—当前输出"的脉冲响应。预览增益绝对值会随时间指数衰减，衰减速度取决于系统动态和权重，通常预览窗口末尾的值已经不到初值的5%。

第三步，在线或离线执行控制律：

[ u[k] = -K_I w[k] - K_x \mathbf{x}[k] - \sum_{i=1}^{N_L} K_{prev}[i] \cdot p^{ref}[k+i] ]

这段控制律在Matlab代码里通常就是一个for循环，逐时刻迭代。对离线WPG来说，循环跑完就得到整段CoM轨迹；在线应用中，每次采样只计算当前步的控制量，预览窗整体向前滑动。两者的数学本质完全相同，只是计算时机不同。

3.4 输出与可视化

输出通常包含CoM位置、速度、加速度和实际ZMP。我会建议多画两张图：一张是CoM-ZMP轨迹对照图，另一张是相位图（CoM位置对速度）。前者能直观看到ZMP是否始终落在支撑多边形内，后者能看出重心运动是否平滑、有没有突变。一个典型的检查项是：稳态直行时，CoM位置与ZMP参考的偏差应当收敛到很小的稳态误差范围内，而CoM速度曲线不应出现尖峰。

4. 从离线生成到实际行走：除了把轨迹算出来，还差什么

很多人在仿真里用这套WPG生成了轨迹，放到真机或高保真仿真器里却发现根本走不起来。问题往往不在WPG本身，而在于从"理想轨迹"到"实际执行"之间的几个关键环节。

4.1 落脚点的运动学反解

WPG输出的只是CoM轨迹和迈步的时序，真正要驱动关节，还必须完成落脚点规划和逆运动学（IK）。这里有一步容易遗漏：落脚点的参考ZMP是从步态规划器给定的足端轨迹独立生成的，它与CoM轨迹必须在同一个时间基准上严格对齐。如果步态周期、双支撑相占比等参数在两层之间没有严格同步，走起来就会出现"重心已经过去了、脚还没跟上"的漂移感。

4.2 姿态控制与ZMP反馈校正

WPG生成的CoM轨迹是开环动力学意义上的稳定，但它不免疫模型误差和扰动。真机上的尘埃在于：实际ZMP不完全等于参考ZMP，因为地面高度、摩擦、腿部惯量、关节柔性都会造成偏差。工程上常见做法是加一层ZMP反馈控制器：实时估算实际ZMP（通常借助六维力传感器），用一个比例-积分环节修正髋关节（或踝关节）的位置增量，让实际ZMP紧紧咬住参考ZMP。预览控制的功劳在于，它让CoM轨迹"有预知能力"，反馈修正只需要处理小范围的偏差，而不用从零开始动态调整重心。

4.3 状态估计与初始化

这是最容易踩坑的地方。WPG生成轨迹时是从一个特定初始状态（一般是静止直立、CoM速度为0）出发的。如果机器人实际起步时的CoM位置、速度与参考初值不一致，直接套用开环轨迹，机器人第一步就会踉跄甚至摔倒。因此实际代码一定要在起步前做初始化对齐：要么缓慢调整初始姿态，让机身进入WPG的初始状态窗口；要么修改参考轨迹的初始条件，使它与当前真实状态匹配。不少开源代码里都有一小段"起摆"（start-up motion）逻辑，本质就是为了这个目的。

5. 那些年我调预览控制踩过的坑

最后分享几个实打实的调试经验，包括我自己摔过的跟头，和实验室师弟妹反复问过的高频问题。

5.1 权重调参：别让误差项"压死"输入项

预览控制的性能极度依赖权重 ( Q_e )、( Q_x )、( R ) 的配比。一个极端案例：某师弟把 ( Q_e ) 调得特别大，希望ZMP跟踪误差越小越好，结果CoM加速度剧烈震荡，控制量接近饱和，整个轨迹在高频振动。原因很直观：误差权重太大，系统为了死咬参考ZMP，不惜疯狂加速重心，反而违背了"平滑稳定"的初衷。我的经验是先让 ( R ) 保持一个适中值，逐步增大 ( Q_e )，观察CoM加速度的最大值和ZMP误差的均方根，找到误差与平滑度的平衡点。同时矩阵 ( Q_x ) 中位置和速度的权重分开调，速度权重过大会让重心轨迹变"迟滞"，过小则会在起步和停止时出现超调。

5.2 双支撑相的ZMP参考别做成跳变

入门代码里最常见的简化做法，是让ZMP参考在双支撑相内线性地从后脚过渡到前脚。线性过渡本身没错，但很多人在双支撑相结束后直接切换到单支撑参考值，在切换点留下一个斜率突变。预览控制能过滤掉同类突变，但如果你发现CoM轨迹在切换点附近有小幅振荡，去检查一下参考ZMP的斜率，而不是怀疑控制器算错了。我通常把过渡段做成三次多项式，首末斜率都归零，实际效果会明显更干净。

5.3 控制周期和预览窗口的匹配

如果你的板子算力有限，控制周期只能做到20ms，请务必重新离散化系统矩阵，而不是把5ms代码里的A、B矩阵直接拿来用。离散化系数不同，最优反馈增益也有差异，直接用错矩阵轻则性能退化、重则系统发散。另外预览步数要与控制周期联动：预览时间窗口保持不变，控制周期放宽后，预览步数要相应减少，否则远处的参考权重还残留着一段不可忽略的非零增益，白白增加了计算量，还把远期轨迹的扰动带了进来。

5.4 数值病态的排查优先级

如果Riccati方程求解报错或结果异常，我一般按三步排查：先检查系统是否可控（Matlab里直接看可控性矩阵的秩），再检查权重的数量级差是否超过1e10，最后检查是否有非法参数（比如CoM高度为0或负数）。这三个问题在同学们给我的代码里几乎覆盖了九成的情况。

6. 从这份WPG继续往前走的方向

ZMP预览控制WPG不是终点，但它是一块极好的跳板。理解它之后，你可以向几个不同方向延伸：第一，把固定的LIPM扩展为可变高度倒立摆，让机器人能在行走中完成蹲起或上下坡；第二，把单步预览控制升级为完整的多步MPC，直接在线优化落脚点与重心轨迹，这是目前足式机器人运动规划的一个主流方向；第三，引入角动量控制和全身动力学作为反馈补偿，弥补LIPM把机器人抽象为质点的先天局限性。

在Matlab里动手跑一遍ZMP-Preview-Control-WPG，把CoM-ZMP轨迹图画出来，再去改参数、观察变化，你对双足机器人行走的本质理解会上一个台阶。纸上谈兵终究不如亲手调一次参数来得真实。祝各位走稳这一步。

本文还有配套的精品资源，点击获取