简介面向双足机器人行走控制与轨迹规划领域研究者和工程师的开源实现聚焦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}[k1] 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[k1] w[k] (p^{ref}[k] - p[k]) )与原始状态一起组成增广状态[ \tilde{\mathbf{x}}[k1] \tilde{A} \tilde{\mathbf{x}}[k] \tilde{B} u[k] \tilde{G} p^{ref}[k] ]性能指标取二次型[ J \sum_{jk}^{\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 顶层脚本与参数配置代码入口通常是一个参数配置文件或直接写在脚本头部。核心参数包括参数类别典型数值说明控制周期 T0.005 s也决定了离散系统精度CoM高度 zc0.85 m人形机器人重心高度重力加速度 g9.81 m/s²直接进入输出矩阵D步长0.3 m单步步距步态周期1.2 s双支撑与单支撑时间分配预览步数 N320~480覆盖2倍步态周期权重 Qe, Qx, R1e6, 1e-3, 1e-6需要反复调试注意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_{i1}^{N_L} K_{prev}[i] \cdot p^{ref}[ki] ]这段控制律在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轨迹图画出来再去改参数、观察变化你对双足机器人行走的本质理解会上一个台阶。纸上谈兵终究不如亲手调一次参数来得真实。祝各位走稳这一步。本文还有配套的精品资源点击获取