两足机器人ZMP预览控制行走模式生成Matlab实现解析

简介：双足机器人的ZMP预览控制行走模式生成在步行稳定性研究中十分关键，这套开源实现恰好提供了可直接学习的完整代码。资源面向自动控制、机器人工程等方向的学生与研究者，帮助理解如何通过预览控制计算增益矩阵，并用前馈控制生成稳定步态。源代码由Matlab和Python共同构成：Matlab负责求解预览控制参数，Python负责行走仿真，并输出X/Y方向上的ZMP与CoM轨迹对比图，整个过程清晰展示了从控制参数到轨迹结果的实现路径。压缩包共16个文件，包含6个Matlab脚本、1个Python脚本、1个Jupyter Notebook交互式说明、1个.mat参数文件、多张bmp/png结果图以及README文档；其中Matlab脚本用于增益矩阵计算与轨迹测试，Python脚本用于前馈控制仿真，ipynb提供交互式演练，图片展示ZMP/CoM轨迹结果。整体仅109KB，轻量且结构清晰，便于阅读和二次修改。目前已有630人学习下载。通过该资源，读者可以快速复现双足步行模式生成实验，掌握零力矩点与质心轨迹的约束关系、预览控制在稳定性控制中的应用方法，是兼具教学与工程参考价值的开源实现。

1. 两足机器人的ZMP预览控制WPG：行走模式生成的落地门槛

两足类人机器人的行走模式生成（WPG）里，ZMP预览控制是被讨论最多的方案之一。我第一次拿到这套离散控制Matlab代码时，只改了一个参数——把预览步数从默认的16改成8，仿真结果是前两步正常、第三步直接向前扑倒。那一刻才意识到WPG不只是算一条质心轨迹，更是离散控制与时序逻辑的配合。这套代码解决的核心问题是：给定脚掌落点和期望ZMP参考轨迹后，如何通过预览控制提前规划躯干质心运动，让每一步的ZMP都落在支撑多边形内。适合机器人控制方向的研究生、做足式机器人预研的工程师，以及把行走仿真当黑匣子、想拆开看细节的人。

2. 从ZMP到预览控制：为什么WPG要“偷看”未来N步

2.1 ZMP、COP与质心投影：稳定判据先分清

ZMP的全称是Zero Moment Point，定义是：地面上存在一个点，地面对机器人反作用力的合力绕该点的水平分量为零。说人话就是，你在脚底找一点，地面反力在该点上不产生绕水平轴的翻转力矩，这一点就是ZMP。稳定行走的硬性要求是ZMP始终落在支撑多边形内部，一旦ZMP越出支撑边界，脚掌就绕边界翻转，机器人跟着倒。

初学者最容易把ZMP和COP混在一起。COP是实际压心，是压力传感器测出来的合力的作用点。静态站立时ZMP和COP重合；一旦开始动态行走，质心加速度产生惯性力，ZMP会偏离COP，两者关系由动力学方程约束。另一个容易混的是质心投影，静态下三者在一点，动态下质心投影可能不在支撑多边形内，不能拿它当稳定判据。

强调一点：ZMP判据适合刚性地面上的足式机器人，落脚点在控制层面可以被当作“硬约束”。如果在柔软地面或坡度上，ZMP的物理意义会弱化，那是另一套问题，不在本篇范围内。

2.2 预览控制的内在逻辑：倒立摆模型的离散化

预览控制（Preview Control）的思想并不复杂：与其等ZMP误差出现后再反馈修正，不如把未来一段时间的期望ZMP轨迹当作前馈信息，提前调整质心运动。类比一下开车：只盯着当前位置纠正方向盘，过弯必甩；提前看弯道曲率，入弯前就减速打方向。预览控制就是这个“提前看弯道”的动作。

实现预览控制前要先选模型。两足机器人全自由度动力学模型维度太高，不适合直接做轨迹生成。最常用的是三维线性倒立摆模型（3D-LIPM），核心假设是：质心高度恒定，支撑腿等效为无质量伸缩杆，水平方向的质心运动由ZMP位置驱动。模型方程简化为：

[ \ddot{x} = \frac{g}{h}(x - p_x) \quad \ddot{y} = \frac{g}{h}(y - p_y) ]

其中 (g) 是重力加速度，(h) 是质心高度，(x,y) 是质心水平位置，(p_x,p_y) 是ZMP位置。水平两轴独立解耦，这让问题大幅简化。注意这个模型的适用边界：质心高度恒定是近似假设，实际机器人行走时质心会有起伏，控制在仿真里加入质心高度约束或吸收抖动的方式处理。

离散化是这套Matlab代码的一个关键动作。连续模型通过零阶保持器离散，推导出如下状态空间方程：

% 线性倒立摆模型的离散化，采样周期 Ts A = [1, Ts, Ts^2/2; 0, 1, Ts; 0, 0, 1]; B = [Ts^3/6, Ts^2/2, Ts]'; % 输入为质心加加速度 dd_x C = [1, 0, -h/g]; % 输出为 ZMP 位置，来自运动学关系 p = x - h/g * ddx

逻辑说明：状态变量取质心位置、速度、加速度，输出取ZMP位置。(C) 矩阵里的 (-h/g) 正是倒立摆模型的动力学约束——质心加速度产生ZMP偏移量。系统矩阵 (A) 是积分链结构，对应“位置→速度→加速度”的积分关系，这是预览控制的标准建模方式。

参数说明：采样周期 (T_s) 直接决定离散精度，常见取值是 0.005~0.02 秒。(T_s) 太大，ZMP轨迹锯齿感明显；太小，预览步数N要对应加大才能覆盖同样时间窗，计算量上升。

得到离散模型后，预览控制解一个有限时间最优控制问题。代价函数包含两项：ZMP跟踪误差平方和、质心加加速度的加权平方和。前者让轨迹跟得紧，后者限制质心运动的激烈程度——加加速度太大意味着电机力矩冲击大。求解这个优化问题得到一组反馈增益 (K) 和一组随时间衰减的预览增益 (F_i)，每一步控制量为：

[ u(k) = -K x(k) + \sum_{i=1}^{N} F_i , p_{ref}(k+i) ]

前一项是状态反馈，后一项是所有未来N步参考ZMP的加权和，(F_i) 离当前时刻越远越小，这正是“预览”含义的数学体现。

2.3 三个反馈增益与预览步数：参数背后的控制意义

预览控制里最常被调的是三个增益和预览步数。三个反馈增益分别对应位置、速度、加速度的权重大小，由LQ调节器求出。它们不是随便给的数字，而是由 (Q)、(R) 权重矩阵决定的：

% LQ 增益求解（离散时间） Q = diag([1, 0, weight_ddx]); % 状态权重：位置误差、速度误差、加加速度 R = 1e-6; % 控制输入权重 [K, ~, ~] = dlqr(A, B, Q, R); % 离散LQR

逻辑说明：(Q) 矩阵对角线上的三个值，对应状态变量（位置、速度、加速度）在代价函数中的权重。位置权重大→ZMP跟踪更紧；加速度权重大→质心运动更平滑。(R) 是控制输入权重，通常给得很小，因为期望的是输出跟踪而不是限制控制量本身。

参数说明：(weight_ddx) 是调参重心。默认值一般取 1e-5 到 1e-3 量级。如果你发现生成的ZMP轨迹抖得厉害，把 (weight_ddx) 调大两个数量级试试；如果ZMP误差大到超出支撑多边形，先把位置权重调上去再看效果。

预览步数N是另一个关键参数。N决定“看多远”：

血泪经验：预览步数不是越大越好。预览增益序列随时间指数衰减，超过一定步数后增益已经趋近于零，再加步数只增加计算量、不改善性能。我现在的习惯是先用公式 (N \approx 2 \times 步态周期 / T_s) 估算一个初始值，再用仿真结果微调。预览时长至少要覆盖一个完整步态周期的 1.5 倍，否则跨步切换瞬间的ZMP参考突变会来不及响应。

3. Matlab代码包拆解：数据结构、主循环与第一次仿真

3.1 代码目录与数据流：拿到压缩包先看哪五个文件

这套系统开源，代码包的目录结构大概率长这样——不同打包方式文件命名有差异，但角色是固定的：

数据流是一根链：config参数 → ZMP参考生成器产生出未来N步参考轨迹 → 预览控制核心在每一采样周期算控制量 → 积分状态变量得到质心位置/速度/加速度 → 可视化整理曲线。中间有一个容易被忽略的环节：支撑相位切换信号也会传给ZMP参考生成器，因为参考ZMP在双脚支撑相和单脚支撑相的形状不同。

拿到压缩包后，先把文件列一遍，确定主脚本入口。然后用Matlab打开主脚本逐行读，看它在调哪些子函数。如果主脚本里写死了数值参数，优先把这些参数抽到config文件里，后面调参不用反复翻主循环。

3.2 支撑相位切换：单脚与双脚的时序逻辑

两足行走不是简单地把两条腿交替往前迈——在一个步态周期里，支撑相分为单脚支撑（SSP）和双脚支撑（DSP）两个阶段。单脚支撑时只有一条腿着地，ZMP参考是恒定的（稳定在支撑脚中心附近）；双脚支撑时两条腿同时着地，ZMP参考从后脚平滑过渡到前脚。这个过渡怎么做，是WPG好坏的分水岭。

最常见的做法是用五次多项式做ZMP参考插值，让位置、速度、加速度都连续。如果只用线性插值，位置连续但速度不连续，ZMP参考在切换瞬间出现斜率突变，预览控制跟踪不连续信号时会产生加速度尖峰，体现到机器人上就是上身前后晃。参考ZMP生成的一段示意代码：

% 双脚支撑期ZMP参考过渡：五次多项式插值 function zmp_ref = gen_dsp_ref(start_pos, end_pos, t_norm) % t_norm: 归一化时间 0~1，用于双脚支撑期 % 五次多项式系数：位置和速度在端点处连续 coeff = [6*t_norm^5 - 15*t_norm^4 + 10*t_norm^3, 0, 0]; zmp_ref = start_pos + (end_pos - start_pos) * coeff(1); end

逻辑说明：五次多项式的关键性质是两端的一阶导、二阶导都为零，也就是说过渡起点和终点的速度、加速度都归零。这样衔接单脚支撑期（参考ZMP恒定）时，信号本身和它的变化率都能平滑过渡。

参数说明：(t_norm) 是归一化时间，双脚支撑期在最终仿真代码里按实际时间长度换算。双脚支撑时间占比通常取步态周期的 20%~30%。占比太小，ZMP过渡太急；占比太大，行走周期拉长，质心速度曲线呈明显波浪形。

时序逻辑上，一个步态周期按“SSP → DSP → SSP”循环。注意脚掌切换的瞬间要更新支撑多边形坐标，预览控制主循环里每一步都要根据当前相位重新查支撑域边界，否则稳定判据用的是旧多边形，导致踩坑——这个留在第5章详细说。

3.3 跑通第一个仿真：从参数脚本到画ZMP轨迹

第一次运行建议不要改任何参数。先复现默认结果，确认代码在你当前的Matlab版本上能跑通。有些老代码用了旧版函数或绘图语法，新版本会报错，先定位这类兼容性问题再谈调参。标准运行步骤如下：

% 第一步：在Matlab中切换到代码包目录 cd('/path/to/zmp_preview_control_wpg'); % 第二步：运行参数配置脚本（如果config是脚本形式） run('config.m') % 第三步：调用主生成函数，得到质心轨迹和时间轴 [time_axis, c_pos, c_vel, c_acc, zmp_actual, zmp_ref] = wpg_generate(cfg);

逻辑说明：(wpg_generate) 是核心封装函数，输入是结构体 (cfg)，输出包括时间轴、质心位置/速度/加速度、实际ZMP和参考ZMP。(zmp_actual) 不是传感器测量值，而是由质心加速度按倒立摆模型反推出来的ZMP——(p = x - h/g \cdot \ddot{x})。仿真的控制效果好不好，就是对比 (zmp_actual) 和 (zmp_ref) 的跟随程度。

参数说明：(cfg) 结构体里最关键的四项是：采样周期 (cfg.dt)、预览步数 (cfg.N)、质心高度 (cfg.h)、步态周期 (cfg.period)。首次运行只用默认值。运行完直接画结果：

% 可视化：参考ZMP和实际ZMP叠画 figure; plot(time_axis, zmp_ref, 'k--', 'LineWidth', 1.2); hold on; plot(time_axis, zmp_actual, 'r-', 'LineWidth', 1.0); xlabel('时间 (s)'); ylabel('ZMP 位置 (m)'); legend('参考ZMP', '实际ZMP', 'Location', 'best'); grid on;

逻辑说明：画图的目的不是“看着好看”，而是确认两点——第一，实际ZMP是否贴着参考ZMP走，偏差过大会导致稳定裕度不足；第二，曲线切换处有无毛刺，毛刺说明支撑相位衔接有问题。x轴不做归一化，直接用秒，方便和步态周期对应起来。

参数说明：如果图里 (zmp_actual) 在每一个步态周期开始处有一小段明显迟滞，不需要急着调增益——先把预览步数 (N) 加大再看。预览控制对参数的敏感度，按重要性排序大约是：预览步数 > 质心高度 > 采样周期 > 增益权重。这也是调参顺序的依据。

提示：第一次跑通后，把默认结果图截图存档。后面改任何一个参数，都跟这张基线图对比，能快速判断改动是朝对的方向还是反的方向。

4. 参数调节实战：从默认配置到稳定行走的三步调整

4.1 步态参数与物理约束：不是所有数值都能填入代码

行走模式生成的参数不是独立的物理量，它们被若干约束绑在一起。最核心的一个约束：步长不能超过支撑多边形半宽的某个安全倍数。假设脚掌宽度是0.08米，支撑多边形半宽约0.04米，那ZMP参考的最大横向偏移不能超过0.035米——留出0.005米的安全裕度。纵向同理，步长决定了ZMP前移量，但ZMP超前于质心投影的极限由倒立摆动力学和质心高度共同决定。

列一张核心参数表，单位是工程上常用的米/秒/秒：

| 支撑域安全裕度 | 0.005 m | 扎实现实取值 |

质心高度 (h) 不是想改就能改的——它由机器人腿长和关节结构决定。改小了，倒立摆等效模型更“矮胖”，ZMP响应变迟钝；改大了，模型更“高瘦”，稳定性变差但对ZMP跟踪更灵敏。如果不是在改机器人硬件，(h) 保持默认。

4.2 调参顺序：先周期与步长，再预览步数，最后动增益

调参最忌讳一上来就动 (Q)、(R) 矩阵里的增益权重。增益是控制层参数，基座的行走层参数还没定，调增益没有意义。我在项目里摸索出的顺序，三步走：

第一步，定步长和步态周期。这两个参数决定ZMP参考的基本形状——梯形波的平顶高度和过渡斜率。步长设0.2米、周期设1.0秒，让ZMP参考梯形波轮廓清晰。确认双脚支撑占比0.25，即过渡期为0.25秒。

第二步，按公式算预览步数初值。预览时长要覆盖至少1.5个步态周期：(N = 1.5 \times period / dt = 150)。150步对预览控制来说偏大，可以先用 (N = 50) 试跑，时间窗0.5秒对应半个步态周期多一点——你会发现ZMP误差偏大，但轨迹趋势正确。再把N逐步加到100上下，观察误差是否收敛。

% 推荐做法：预览时长锁定为步态周期的1.5~2倍 preview_horizon = 1.8 * cfg.period; % 1.8倍步态周期 cfg.N = round(preview_horizon / cfg.dt); fprintf('预览步数 N = %d\n', cfg.N);

逻辑说明：这段代码在参数脚本里做动态计算，而不是手写固定N。好处是换步态周期时不需要再回头改N，减少人为疏忽导致的参数不配对。

参数说明：(preview_horizon) 的倍数取1.8是实测折中——再大收益递减，再小跟踪延迟增加。如果你跑出来的ZMP误差曲线在每一步的起始处有“拖尾”，优先加这个倍数到2.0，而不是去调增益。

第三步，等ZMP误差降到支撑域半径的20%以内，才开始微调增益。加加速度权重 (weight_ddx) 从1e-5起步，每次乘10倍看趋势：

• 误差大、抖：减小 (weight_ddx)，让控制更“敢动”
• 误差小但质心加速度毛刺多：增大 (weight_ddx)，让运动更平滑
• 位置误差一直压不下来：把状态权重 (Q(1,1)) 从1调到10

常见误区是把增益调参当成玄学，其实趋势判断有规律：ZMP误差和质心平滑度是两个对顶的指标，调参本质是在两者之间找平衡点。

4.3 判断稳定性的可视化准则：不要只看ZMP在不在框内

判断WPG输出是否合格，最直觉的方法是看ZMP曲线是否还在支撑多边形内。但这个标准太宽松——ZMP贴着边界走，虽然没越界，但机器人稍微受到一点干扰就摔。工程上判断合格要看三个层面：

第一个层面，ZMP误差幅值。参考ZMP和实际ZMP之间的最大偏差应小于支撑多边形半宽的20%。如果支撑半宽是0.04米，那允许的最大偏差是0.008米。超过这个数，说明跟踪太松，稳定裕度不够。

第二个层面，ZMP轨迹平滑度。看ZMP曲线速度（一阶导数）有没有极限突变。参考ZMP在双脚支撑过渡段的起点和终点处，前两阶导数应为零，实际ZMP曲线在这些位置不应该有折角。

% 计算ZMP速度，检查是否有跳变 zmp_vel = diff(zmp_actual) / cfg.dt; jumps = find(abs(diff(zmp_vel)) > 0.5); % 经验阈值：m/s^2 if ~isempty(jumps) warning('检测到可能的ZMP速度跳变，采样点索引: %s', num2str(jumps')); end

逻辑说明：(diff) 函数求差分相当于数值微分，(zmp_vel) 是ZMP移动速度。对ZMP速度再求差得到加速度，如果加速度在某点超过阈值，说明轨迹不平滑。实际行走中这个跳变会直接反映为上身前倾后仰的震荡。

参数说明：0.5 m/s² 这个阈值是经验值，根据不同机器人尺寸可能差异较大——脚掌短的机器人对ZMP敏感度更高，阈值要收紧到0.3。先把默认阈值跑一遍，如果没触发警告再把阈值收紧，直到触发为止，那才是你当前系统真正的平滑边界。

第三个层面，质心加速度峰值。质心加速度和关节力矩强相关，峰值过大会导致电机饱和。检查整个时间轴上的最大加速度，如果超过机器人电机能提供的平移加速度上限（通常由腿长和电机峰值力矩估算），说明步态参数太激进，需要放慢步态周期或缩小步长。

5. 避坑与排查：跑WPG demo最容易翻车的五个点

5.1 预览步数改小导致起步即摔

现象：默认参数跑通后，想“精简计算”把N从32改成8，重新仿真，机器人前两步正常、第三步向前扑倒。

原因：预览时长 (N \times dt) 只有 0.08 秒，连一个步态周期的 1/10 都不到。预览控制的前馈项看到的信息太少，ZMP参考从后脚过渡到前脚的突变来不及预防。更本质地说，预览增益序列 (F_i) 里有能量的部分集中在未来 1~2 步，N=8 时序列被截断，相当于把一个本来有长尾巴的滤波器截短了，相位响应失真。

解决：N 的下限取 (period / dt \times 1.5)，低于这个值不要硬跑。在我的项目里，步态周期1.0秒、采样0.01秒时，N最小取150才稳。嫌计算量大可以改采样时间，而不是砍预览步数。

5.2 采样时间加大产生锯齿ZMP曲线

现象：为了加快仿真速度，把采样时间从0.01改成0.05，跑出来的ZMP轨迹在过渡段有锯齿。

原因：倒立摆模型离散化后，预览增益序列 (F_i) 的衰减速度与采样时间相关。采样时间变大，离散系统失真，高频分量跟踪能力下降，ZMP轨迹在双脚支撑过渡段的高斜率区出现阶梯状。

解决：采样时间按物理执行器的控制频率来定——仿真里可以小，但与实物耦合时不能小于底层控制周期。我一般把采样时间固定为模拟器控制周期的整数倍，而不是随手改大。如果你只是为了快速验证逻辑，可以用0.02到0.05查看趋势，但最终生成轨迹必须回到0.01级采样重跑。

5.3 支撑多边形用了矩形近似导致误判稳定

现象：可视化结果里ZMP明明在画的矩形框内，但机器人仿真还是倒了。

原因：大多数WPG代码默认用矩形近似支撑多边形——长等于脚长、宽等于脚宽，然后直接在矩形框内判断ZMP是否越界。真实脚掌的支撑边界是凸包，特别在踝关节外偏、脚趾圆角等异形结构下，矩形比凸包“宽容”得多，存在ZMP已出凸包但还在矩形框里的盲区。

解决：跑仿真前，确认支撑多边形是凸包还是简化矩形。如果代码里写死了矩形，手工按足底实际轮廓生成凸包坐标替换掉。生成凸包的通用做法是，取脚掌边界上所有关键点，用 (convhull) 函数求凸包：

% 用足底关键点生成真实支撑多边形 foot_corners = [0, 0.04, 0.03, 0; % 左前、左后、右前、右后示例 -0.06, -0.02, -0.02, -0.06]; hull_idx = convhull(foot_corners(1,:), foot_corners(2,:)); polygon = foot_corners(:, hull_idx);

逻辑说明：(convhull) 返回凸包顶点的索引顺序，保证多边形边界是闭合凸多边形的逆时针/顺时针序列。拿这个多边形去替代代码里的矩形近似的支撑域。

参数说明：脚掌坐标以踝关节投影点为原点。不同脚型坐标差异极大，这些值必须来自你的机器人足底模型，不能照抄。

5.4 第一步就猛冲：初始状态没和预览控制对齐

现象：仿真开始时质心速度不为零，第一步质心轨迹直接冲出去一大截，后续才慢慢拉回来。

原因：预览控制要求提供初始状态 (\bigl(x(0), \dot{x}(0), \ddot{x}(0)\bigr))。如果主脚本里给的是静止变量但跟模型期望初始条件不一致，控制量输出的第一拍会产生一个大幅修正量。

解决：让WPG从双脚静止站立开始，用前几步逐步过渡到动态行走。常见做法是设置一个斜坡式ZMP参考：开始0.5秒内ZMP从零线性过渡到第一个落脚点中心，给预览控制一个“缓冲期”。或者直接在初始化时把状态设定为 (x(0)=0, \dot{x}(0)=0, \ddot{x}(0)=0)，同时把参考ZMP从当前位置出发——保证系统处于平衡点附近。

5.5 Matlab新版本运行报错

现象：代码在旧版本上正常，拿到新版Matlab报函数不存在或输出参数过多。

原因：老代码使用了一些旧版专属函数或语法。例如输入参数解析器在新版中要求更严格，或某些矩阵分散函数在近几个版本被移除。

解决：先运行 which 检查函数是否存在：

which dlqr which zeros

如果缺失就找替代方案。配置文件里的函数句柄不要用字符串形式调用，改用 str2func 显式构造。遇到欧拉角相关的矩阵运算，检查是否用了被移除的旋转矩阵生成函数，换成旋转向量或四元数接口。我的习惯是：拿到代码先花10分钟做版本兼容性静默检查，再谈跑仿真的事。

6. 验证行走生成质量：从曲线到关节指令的三步走

跑出ZMP曲线只是第一步。要确认这个行走模式能真正落到机器人上，我习惯再走三步验证。第一步是算稳定裕度：ZMP实际轨迹到支撑多边形边界的最小距离包络，小于0.005米就认为偏危险。用代码实现就是逐点计算多边形距离，找出全局最小值：

min_margin = inf; for i = 1:size(zmp_actual, 1) d = point_to_polygon_dist(zmp_actual(i,:), current_polygon); min_margin = min(min_margin, d); end if min_margin < 0.005 error('稳定裕度不足：最小边界距离 %.4f m', min_margin); end

第二步是把质心轨迹反解成关节角度，检查踝关节和髋关节角度是否超限。先做一个简化的腿部逆运动学：已知质心位置和支撑脚位置，用几何关系求膝关节弯曲角度。如果膝关节角度在步态周期内超过150度，说明步长相对腿长过大，行走模式实际不可执行。

第三步是检查质心加速度脚本中生成的ZMP速度是否与机械结构产生的运动速度匹配。如果生成的质心横向速度峰值超过结构速度阈值，说明步态周期太短。这里给一个快速的验证规范化流程：固定步态周期、固定步长、固定双脚支撑比例，三次仿真逐步收紧阈值，看稳定裕度曲线是收敛还是发散。

从那以后我每次拿到一套新的行走模式生成代码，都强制走一遍三件事：第一看预览步数是否与步态周期匹配，第二确认支撑多边形是凸包还是矩形近似，第三检查初始状态是否与预览控制器对齐。这三个问题占掉了我在项目里遇到的八成翻车现场。希望帮到你。

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