企业官网建设流程全解析

简介：这是一份面向机器人控制与仿人步态研究者的开源实现，聚焦两足类人机器人的ZMP预览控制行走模式生成。代码采用Matlab与Python混合编写：Matlab负责计算预览控制增益矩阵，Python则基于前馈控制完成系统仿真，并输出x、y方向上的ZMP与CoM轨迹对比结果，便于理解从理论推导到数值验证的完整链路。

资源包共16个文件，以6个Matlab脚本为核心算法实现，辅以参数文件、Python仿真脚本、交互式分析笔记本及说明文档，另含轨迹结果图片和开源许可证。整体压缩包仅109KB，结构精简，适合机器人专业学生或工程师快速上手复现。

目前已有630人学习使用。下载后可直接获得增益计算、轨迹生成、前馈控制仿真等关键代码，并参照README与配套文章的符号约定深入理解算法细节，对开展双足步行控制研究具有实用参考价值。

1. 离散ZMP预览控制的Matlab代码包：两足行走模式生成，这套资源能直接改参数跑通

两足机器人和类人机器人步态规划里，ZMP预览控制（ZMP Preview Control）一直是个绕不开的经典方案。很多初次接触的人看了论文觉得公式能看懂，一上手写Matlab就卡在离散化、矩阵求解和预览增益的调参上。这份代码包解决的正是这个问题：它把ZMP-Preview-Control-WPG完整拆开，用离散控制的方式实现了行走模式生成器，输入脚印坐标和步态参数，输出平滑稳定的质心（CoM）轨迹和ZMP轨迹。资源是系统开源形式的Matlab工程代码，适合正在做足式机器人步态控制的研究生、入门足式机器人算法工程师，也适合拿标准算例对比自己数学推导的人——代码里每一步的矩阵维度、增益计算和坐标系定义都值得反复对照。

2. 先立住理论：ZMP预览控制的模型、离散化与三个核心参数

2.1 为什么是ZMP与三维线性倒立摆：选型理由与模型边界

ZMP（Zero Moment Point，零力矩点）的定义是地面反作用力合力作用点，在这个点上水平方向的倾覆力矩为零。只要ZMP落在支撑多边形内部，机器人就不会绕支撑脚翻倒。这和重心投影点（CoM投影）是两回事：CoM投影在支撑多边形内不一定稳，ZMP才直接和地面接触力相关。所以步态规划的目标一般写成：给定期望ZMP轨迹，反解出CoM轨迹，让实际ZMP能跟踪上期望值。

直接对完整的多刚体动力学做轨迹规划计算量太大，也难保证实时性。常见做法是三维线性倒立摆模型（3D Linear Inverted Pendulum Model, 3D-LIPM），约束条件是把质心高度固定在常数，这样ZMP和CoM在水平面上的动力学就变成了两套线性方程：

• 方向：( \ddot{x}{CoM} = \omega^2 (x{CoM} - p_x) )
• 方向：( \ddot{y}{CoM} = \omega^2 (y{CoM} - p_y) )

其中 ( \omega = \sqrt{g / h} )，h是固定的质心高度。写成这个形式之后系统变成线性的，后面直接用矩阵方式做无穷时域最优控制或预览控制就方便了。预览控制的核心思路不是只考虑当前时刻的误差，而是把未来N步的ZMP参考值全部纳入代价函数，提前规划当前的CoM加速度。这个预防性的前瞻机制就是和普通状态反馈控制的本质差别——ZMP Preview Control这个名字的由来。

需要知道这个模型的边界，不然用的时候容易翻车：

• 质心高度必须是定值，实际机器人蹲起过程不能直接用
• 没有考虑角动量变化，躯干转动较快的时候模型会失真
• 模型是线性的，地面不能明显倾斜

这套边界决定了代码包的适用范围：平地行走、速度不太高的双足步态，前向和侧向分开解算。

2.2 离散化公式与预览增益计算：从连续系统到可编程的差分方程

连续模型的状态量取 ( [x, \dot{x}, p_x]^T )，即CoM位置、CoM速度、实际ZMP位置。连续状态方程为：

[ \frac{d}{dt} \begin{bmatrix} x \ \dot{x} \ p_x \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 \ \omega^2 & 0 & -\omega^2 \ 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} x \ \dot{x} \ p_x \end{bmatrix} + \begin{bmatrix} 0 \ 0 \ 1 \end{bmatrix} u ]

这里控制输入u是ZMP的变化率。为什么要引入ZMP变化率而不是直接用ZMP作为输入？因为在最优控制框架下，把ZMP的“变化率”作为控制量，优化目标里就可以显式惩罚ZMP的剧烈变化，生成出来的轨迹更平滑，也更接近实际执行器能跟踪的效果。

离散化采用零阶保持（ZOH），采样周期为Δt（代码里一般用0.005秒或0.01秒）。离散状态转移矩阵需要用矩阵指数：

[ F = e^{A \cdot \Delta t}， G = \int_0^{\Delta t} e^{A \cdot \tau} d\tau \cdot B ]

在Matlab里直接调expm(A*dt)算F，然后通过增广矩阵法算G。注意不要自己手写展开泰勒级数只取前三项，那样在采样周期偏大时误差会累积到轨迹上。常见做法是构造增广矩阵：

% 连续系统矩阵 A = [0 1 0; w^2 0 -w^2; 0 0 0]; B = [0; 0; 1]; C = [1 0 -1]; % 输出是CoM位置与ZMP位置的差 % 增广矩阵离散化，一次性得到F和G Aug = [A, B; zeros(1,4)] * dt; ExpAug = expm(Aug); F = ExpAug(1:3, 1:3); G = ExpAug(1:3, 4);

逻辑说明：这里增广矩阵的维度是4×4，最后一行只有0和1，对应控制输入积分。expm计算的是矩阵指数而非逐元素指数，这步是离散化是否准确的分水岭。参数说明：dt的取值直接决定预览增益的数值，工程上对比过0.005秒和0.02秒两个采样，0.005秒出的轨迹平滑度明显更好，但计算耗时翻倍，离线生成行走模式时不敏感，实时用的场景建议0.01秒起步调。

预览控制的代价函数写成：

[ J = \sum_{j=k}^{\infty} \left( e_j^T Q e_j + \lambda \Delta u_j^2 \right) ]

其中 ( e_j ) 是输出误差（实际ZMP减去参考ZMP），Q和λ是权重。求解离散代数黎卡提方程（DARE）得到反馈增益K，再算预览增益向量P。P的长度等于预览步数N，matlab里用dlqr求解黎卡提方程，然后按标准递推公式计算P序列，这步代码包里已有实现，不需要自己推导，但要知道这两个关键点：

• 预览步数N设为未来参考ZMP的采样点数，N×dt等于前瞻时间，前瞻时间建议覆盖1～2个单步周期
• Q和λ的比值控制ZMP跟踪精度与CoM加速度幅度的折中，Q偏大轨迹跟踪更紧但质心加速度尖峰更明显，λ偏大轨迹更平滑但ZMP误差变大

3. 代码包结构与主流程：从脚印序列到CoM轨迹的完整链路

3.1 代码包文件布局与每个文件的作用

拿到代码包后建议先按文件功能划分为三个层次：参数定义层、算法计算层、仿真可视化层。这个代码包的典型文件清单如下：

主入口的调用链一般是：

% main_wpg.m 主流程 param = set_parameters(); % 1. 读取全部参数 foot = make_footprint(param); % 2. 根据步长/步宽生成脚印 refZmp = calc_reference_zmp(foot, param); % 3. 生成未来参考ZMP序列 [K, P] = calc_preview_gain(param); % 4. 求解反馈与预览增益 [comPos, comVel, zmpOut] = calc_wpg_trajectory(K, P, refZmp, param); % 5. 递推主循环 plot_result(comPos, zmpOut, refZmp); % 6. 结果可视化

逻辑说明：五个步骤的顺序不要随意调换，第2步脚印序列的质量直接决定第3步参考ZMP的平滑性，而第4步的增益计算独立于脚印生成，可以提前缓存好。参数说明：set_parameters.m里的PreViewStep和dt是频率相关的两个参数，改任意一个都要重算calc_preview_gain，gai数不能复用到另一组采样参数上。

3.2 预览增益计算核心段：dlqr与递推公式实现

预览增益的计算是这套WPG的数学核心。标准做法是：先用dlqr求状态反馈增益K，然后根据DARE的解递推预览增益向量P。代码如下：

% calc_preview_gain.m 节选 function [K, P] = calc_preview_gain(param) dt = param.dt; w = sqrt(param.g / param.zH); A = [0 1 0; w^2 0 -w^2; 0 0 0]; B = [0; 0; 1]; C = [1 0 -1]; F = expm(A * dt); G = dt * (eye(3) + A * dt / 2 + A * A * dt * dt / 6) * B; % 近似，实际建议用aug法 Q = param.Q; lambda = param.lambda; [K, S] = dlqr(F, G, C' * Q * C, lambda); % 离散黎卡提方程 % 计算预览增益，N为预览步数 N = param.PreViewStep; P = zeros(1, N); P(1) = -G' * S * G / lambda; % 公式见预览控制原始推导 for i = 2 : N x_bar = (F - G * K)'; P(i) = -G' * S * x_bar^(i-1) * G / lambda; end end

逻辑说明：dlqr返回的K是状态反馈增益，利用的是S矩阵（DARE的对称正定解），S矩阵本身也参与预览增益的计算。预览增益序列P的绝对值通常会随着i增大而衰减，这符合“未来更远处的ZMP参考对当前控制决策影响更弱”的直觉。若是P序列出现增长或振荡，先怀疑离散化矩阵F算错，再检查dt是否过大。

参数说明：这里Q和lambda的数值直接决定P的大小。代码包默认参数Q=1.0，lambda如果取1e-6量级，P的第一个元素会非常大，CoM加速度会出现明显的初始冲击。经验值是让lambda / Q ≈ 1e-6 ~ 1e-8，具体数字和质心高度、步长有关，不是玄学，但要结合参考ZMP的最大斜率来感知判断。

% calc_wpg_trajectory.m 递推核心 简化节选 for k = 1 : N_total u = -K * state(:,k) + P * refZmp(k : k + N_preview - 1)'; state(:, k+1) = F * state(:,k) + G * u; end

逻辑说明：这行就是预览控制和普通状态反馈的全部区别所在——P * refZmp部分把未来N_preview个参考值同时加权求和。如果把这行换成u = -K * state，就不再是预览控制，而是普通的LQR跟踪，轨迹质量会明显变差。参数说明：N_preview建议不小于单个步态周期的采样点数，代码包默认是400步（对应0.005秒采样下的2秒前瞻）。

3.3 参考ZMP轨迹构造：双脚支撑期的处理细节

参考ZMP轨迹的构造往往是被忽视但最影响结果的一环。如果脚印坐标直接作为参考ZMP，轨迹会出现阶跃突变，预览控制器会剧烈输出。常见做法是：在双脚支撑期（Double Support Phase, DSP）让参考ZMP在两只脚的脚心之间线性插值，在单脚支撑期（Single Support Phase, SSP）让参考ZMP固定在该脚踝附近。

% calc_reference_zmp.m 思路节选 % refZmpX(k) = alpha(k) * leftFootX + (1-alpha(k)) * rightFootX % alpha(k)根据当前时刻处于SSP还是DSP线性变化 % DSP期间alpha从0平滑过渡到1，避免阶跃

逻辑说明：alpha系数按采样点线性过渡是最容易实现且质量稳定的方案。想做得更平滑可以换成五次多项式插值，但初始调参阶段建议先用线性过渡跑通一遍，确认ZMP跟踪误差在可接受范围再换。参数说明：单步周期、DSP时长和SSP时长的比例直接影响ZMP参考曲线的斜率，DSP占比一般取单步周期的20%～30%，占比太低会让ZMP轨迹斜率过陡，预览控制器压不住。

4. 避坑手册：预览控制Matlab实现里最容易翻车的五个细节

4.1 增益计算时离散化矩阵F和G用近似公式导致轨迹发散

现象：用dt=0.01采样，前几步轨迹正常，几十步之后CoM轨迹出现锯齿状振荡，周期等于采样周期。

原因：部分实现为了省事会用泰勒展开近似F和G，比如 ( F ≈ I + A dt )，在dt取0.01时这种误差已经足够破坏系统的稳定性。预览控制对离散模型精度非常敏感，G算错相当于系统输入错了半个周期。

解决：统一改用增广矩阵expm方式去算F和G，不要自己写近似展开。代码包里这步用的是expm(4×4增广矩阵)的写法，实测在dt=0.005/0.01/0.02三档采样下都不会出现数值异常。

4.2 预览步数与采样时间的搭配不当导致轨迹迟滞

现象：参考ZMP已经切换方向了，实际CoM轨迹还在往原来方向飘，滞后感明显，像是“反应迟钝”。

原因：预览步数N乘以采样时间dt是预览时间窗。比如dt=0.01，N=200，前瞻时间2秒，这在步频较高（比如单步周期0.8秒）时覆盖了约2.5个周期，信息量足够。但如果N=100而单步周期加到1.6秒，前瞻时间只覆盖半个周期，后面的参考ZMP完全没有利用到，效果和反馈控制差不多。

解决：代码包参数里PreViewStep改到能覆盖至少1.2个单步周期，推荐1.5倍以上。步频越高，N的绝对值要求越小，这是先算单步周期再反推N，而不是固定写死。

4.3 Q和λ权重比例不对，轨迹要么过硬要么过软

现象：λ/Q过小，CoM加速度曲线出现高耸尖峰；λ/Q过大，ZMP跟踪误差超过两厘米，机器人走起来明显重心偏斜。

原因：这个比例决定对“ZMP跟踪精度”和“控制量平滑度”的权衡。ZMP误差单位是米，控制量u单位是米/秒，物理量纲不同，权重直接写1和0.001没有意义。

解决：先确定期望ZMP误差范围，再反推权重。比如允许ZMP误差1cm，那么Q的单位值对应的是1cm误差下的代价，再根据u的典型幅值（大约0.5~2 m/s）去取λ，保证两项代价在同一个数量级。代码包默认参数能跑通，但换机器人参数时这一步必须重新过一遍。

4.4 零状态初始化导致起步瞬间质心加速度冲击

现象：第一帧控制输出特别大，CoM轨迹从零起步时先抖一下，然后才进入平滑段，步态数据前半步看起来像宕机。

原因：预览控制的初始状态是静止站立，zmp输出也是0，而参考ZMP第一步就有较大位置偏移，误差瞬间放大，控制器用最大力气去追。

解决：在参考ZMP轨迹起始处加250~500毫秒左右的“预滚动”段，让参考值先保持为当前站立模式的ZMP位置，再平滑过渡到第一个脚印。代码包的make_footprint里已经有这个缓冲段的实现，不建议把它注释掉。

4.5 侧向（y方向）和前进（x方向）共用同一套预览增益

现象：前进方向走得很稳，侧向轨迹反而有持续抖动，或者反过来。

原因：两个方向在3D-LIPM里的模型结构相同，实质上可以共用增益，但步长和步宽的量级不同。侧向距离变化小，ZMP参考变化斜率通常比前向更陡，同样权重下侧向的跟踪误差更容易偏大。

解决：代码包默认分开了方向。自己改参数时，侧向建议单独调一组λ，一般比前向大2到5倍，让侧向控制更平滑。这是实践中最常被忽略的地方，很多人调了半天前向轨迹，回头才发现侧向问题根本不是一套参数的事。

5. 进阶验证与扩展：把WPG接上仿真之前，先做这三层校验

拿到代码包并不是跑通demo就完事了，至少还要做三层验证，才能确定生成的轨迹真的能用于机器人仿真或实机。

第一层校验：检查输出ZMP和参考ZMP全轨迹的误差包络。运行完calc_wpg_trajectory后绘制zmpOut - refZmp的波形，理想结果是误差在前瞻段结束后收敛到±5mm以内，而且误差带上不存在持续振荡。如果误差带在两三步之后变宽，不是增益问题就是参考ZMP轨迹不平滑，需要回溯。

第二层校验：检查CoM轨迹的加速度连续性。直接对comPos做二阶差分，绘制diff(comPos,2)/dt^2曲线。正常结果应该没有尖峰，曲线基本平滑，如果有尖峰，视觉上就是机器人走路一顿一顿。这层校验能过滤掉大部分假收敛的情况——ZMP误差小不代表轨迹质量高。

第三层校验：做一个离线的外部扰动测试。在某个采样时刻人为改掉参考ZMP（比如在轨迹中间加一个0.02米的阶跃偏移然后恢复），观察预览控制器的反应。这个测试能直观看到预览机制是否在工作：正常结果是在扰动发生之前CoM轨迹就有平滑的前置调整，扰动后快速回归参考ZMP，同时没有剧烈加速度。如果CoM轨迹完全无动于衷，说明预览增益那段有问题，多半是N设得太小。

做过一次三阶倒立摆仿真验证之后，我把这个代码包作为新研究生的入门必跑项目。规定流程是：先改set_parameters.m里的h和dt，重跑main_wpg，确认误差包络在要求范围内；再换一组步长步宽，看轨迹形态是否合理；最后做一次扰动测试，把CoM加速度曲线截图存档。从那以后我每次处理双足步态问题，都会强制先走一遍这套验证流程，再往下接仿真环境。这套从离散化到预览控制全链路的标准实现，搭配代码里的注释和边界条件的处理，确实是快速上手两足WPG的一条直路，希望帮到你。

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