ZMP预览控制与线性倒立摆：双足机器人步态规划的Matlab实现

简介：一套面向双足类人机器人研究者、运动控制方向学生及入门开发者的ZMP预览控制行走模式生成源码，聚焦于行走规划中ZMP与CoM轨迹协调这一核心问题。项目把Matlab与Python搭配使用：Matlab端负责计算预览控制增益、生成ZMP轨迹，Python端通过前馈控制完成CoM轨迹模拟与闭环验证，覆盖从参数求解、轨迹生成到动态仿真的完整链路；附带轨迹对比图和说明文档，方便逐项核对算法输出。压缩包共16个文件，以6个Matlab脚本为主干，另有Python脚本、Jupyter Notebook、MAT格式参数文件、示例图片及README等，整体仅109KB，内容精炼，既能快速读懂核心逻辑，也便于二次开发。目前已有630人学习/下载，尤其适合正在研究双足稳定行走、希望快速搭建ZMP预览控制实验环境，并愿意参照源码逐行理解前馈控制实现细节的读者。

1. 从走三步就摔到稳定行走：ZMP预览控制做了什么

早几年我调一台双足机器人样机，第一版步态是拿正弦函数直接生成质心轨迹，结果机器人站着还能立住，一迈步就歪，平均走三步就要倒。后来换成ZMP预览控制的行走模式生成方案，才算把步态这关过了。这套Matlab代码做的事，正是ZMP预览控制（ZMP Preview Control）的完整落地：输入步长、步周期、质心高度等参数，先规划左右脚落脚序列，再生成参考ZMP轨迹，最后由预览控制器反解出一条平滑质心轨迹送到底层控制。

适合两类人。一类是做双足或类人机器人步态规划、不想从LIPM公式到Riccati方程一步步推的开发者，拿这份代码当参考实现，跑通之后替换自己的模型参数；另一类是想把步态生成模块接进自家框架、需要一份能跑能改的Matlab成品的网友。即便你的机器人是四足或轮足混合形态，预览控制的思路也适用——只要系统能线性化成倒立摆形式，这套流程就能搬。

2. 技术架构与目录结构：LIPM模型、预览控制器和代码布局

在拆开代码之前，先讲清楚两个设计决定：为什么模型要简化，为什么控制器要带预览。这两个决定决定了后面所有文件的长相，也决定了这份代码为什么能作为一套可复用的步态生成模板，而不是只能跑通的一个孤例。

2.1 为什么步态生成要简化成线性倒立摆

双足机器人的完整动力学是多变量强耦合的非线性方程组，直接拿来算步态规划，既没有解析解也拖垮计算效率。业界主流的做法是把机器人抽象成点质量加一根无质量支撑腿，质心高度在整个步态过程中保持恒定，这个模型就是线性倒立摆（LIPM）。

预览控制的思路最早出现在上世纪九十年代的步行机器人研究中，核心思想后来被沿用到四足、轮足混合等多种机器人形态。LIPM只保留水平两个维度的自由度，竖直方向靠“质心高度恒定”锁死，水平方向的动力学与ZMP关系退化成一条很干净的线性方程：

x'' = (g / z_c) * (x - p_x) y'' = (g / z_c) * (y - p_y)

其中x、y是质心水平位置，p_x、p_y是ZMP位置，z_c是质心离地高度，g是重力加速度。这个方程含义直白：ZMP在质心前面，质心就往前加速追；ZMP在质心后面，质心就减速等。机器人走路平衡的本质，是让ZMP始终落在脚下支撑多边形内部，一旦ZMP滑出边界，地面给机器人的反作用力就无法维持当前姿态。

预览控制能成立，依赖这条方程的线性性质。线性系统可以离散化，可以套Riccati方程，可以把未来参考输入叠进控制律。换成非线性的倒立摆模型，这套算法就没有解析解法了，只能上非线性优化，那在线运算量会大一个量级。实际使用时有几个细节容易忽略：z_c取值必须是机器人质心的真实高度，取偏差会导致ZMP跟踪出现系统性偏移；LIPM动力学只描述单脚支撑期的行为，双脚支撑期的重心过渡要靠专门逻辑，这部分代码放在ZMP参考生成模块里。

2.2 预览控制器：反馈之外的前馈通道

普通反馈控制是看到当前误差再纠偏，双足行走只做反馈永远慢半拍。预览控制器多了一条前馈通道：手里握着一整段未来的参考ZMP轨迹，每一步都在用未来N步的信息决定当前给多大加加速度。这有点像开车过弯：老司机看的是弯道出口，不是保险杠前面的白线。

用二次型目标函数量化这个权衡：

J = sum{ e[k]^T * Qe * e[k] + du[k]^T * R * du[k] }

e是当前ZMP跟踪误差，du是质心加加速度（jerk）变化量，Qe和R是权重。目标函数越小，意味着ZMP跟踪越准、质心轨迹越平滑。解出来的控制律分成两半：

u[k] = -K * x[k] + sum{ F[i] * p_ref[k+i] }

前半是状态反馈，看当前质心状态；后半是预览前馈，把未来L步的参考ZMP按F[i]加权进来。F序列就是预览增益，L就是预览窗口长度。窗口越长，轨迹越平滑，但离线计算量和内存占用也越大。

两个权重怎么调，是这套代码里最值得玩味的部分。Qe大，控制器更看重ZMP跟踪，轨迹贴得更紧，代价是质心加速度变化更猛；R大，轨迹更平滑，但ZMP跟踪误差变大，机器人容易走出“发软”的步态。我一般从Qe=1、R=1e-4起步，配合第4章的仿真曲线再微调。注意这里的u是加加速度jerk，不是关节力矩，预览控制只负责几何轨迹，力矩分配是底层控制器的事，模块边界要拎清。预览控制还有个前提：参考轨迹本身不能拍脑袋乱给。ZMP参考如果频繁进出支撑多边形边缘，预览控制器再怎么调权重也救不回来，因为控制力顶不过几何约束。我刚接触这套代码时也踩过这个坑，以为调参能掩盖粗糙的参考轨迹，后来把双脚支撑期的插值改光滑，曲线立刻顺了。

2.3 代码目录与关键文件映射

整套程序沿一条数据流组织：脚序列生成、ZMP参考生成、模型离散化、预览增益求解、轨迹仿真、可视化。拆包后核心文件的大致职责如下：

用数据流的眼光看这个结构，会发现它和真实机器人上的步态生成模块非常接近：脚序列规划器和ZMP参考生成器对应上层导航输出，预览控制器对应在线步态生成，仿真文件对应接进实时控制环之前的离线验证。整套代码按系统开源的方式组织，模块边界干净，改动通常集中在两三个文件里，二次开发不用把整个工程读完。后面改代码也按这条链路从前往后改参数、从后往前查问题，每个文件都不算长，核心逻辑集中在两个generator和一个solver上，适合当脚手架用。

3. 核心模块拆解：从落脚点到平滑质心轨迹的完整链路

这一章把数据流上的四个关键函数逐一拆开。读的时候建议打开代码包对着看，每个函数都不长，但前后依赖关系密：脚序列给ZMP参考提供空间基准，ZMP参考给增益求解提供目标，增益再回过来生成质心轨迹。

3.1 脚踏点序列生成：genFootsteps

脚序列是步态的地基，定义每一步要落在哪里、哪只脚先迈。直线行走时只需要三个参数：步长stepLen、步宽stepWidth、总步数totalSteps。左右脚在x方向交替前进一个步长，在y方向分别保持在步宽正负半侧。代码先从最简单的情况入手，不做转弯、不做变步长，把空间和时间解耦。

% genFootsteps.m % 生成交替前进的左右脚落脚点序列 function steps = genFootsteps(stepLen, stepWidth, totalSteps) steps = zeros(totalSteps, 3); for i = 1:totalSteps % 第i步的x坐标：第一步从原点出发，之后每步前进一个步长 steps(i, 1) = (i - 1) * stepLen; % y坐标按奇偶交替到左脚或右脚，脚踝相对中线偏移步宽的一半 if mod(i, 2) == 1 steps(i, 2) = stepWidth / 2; else steps(i, 2) = -stepWidth / 2; end % z坐标全部贴地，抬脚高度不在这层管 steps(i, 3) = 0; end end

代码逻辑很直白：x方向按步数累加，y方向用mod奇偶切换左右脚，第三维z全置0，抬脚高度属于轨迹插值层的工作。参数说明：

• stepLen：单步步长，单位米，典型值0.1到0.2。步长太大会让单脚支撑期的ZMP偏离支撑中心太远，太小又看不出前进效果；
• stepWidth：左右脚踝间距，典型值0.15到0.25，与机器人髋关节宽度有关，太窄会让ZMP落点接近中线，横向稳定性变差；
• totalSteps：总步数，仿真时设10到20步足够看清趋势。

这段代码只管落脚点位置，不管落脚时序。时序在ZMP参考生成模块里处理。新手最容易犯的错是把步长和周期耦合在一起算，其实代码里步长只管空间、周期只管时间，解耦之后调参数方便很多。如果你想改成转弯步态，也只需要在这里加一个旋转矩阵，其余模块不用动。

3.2 ZMP参考轨迹生成：genZMPRef

有了落脚点，下一个问题是怎么把落脚点位置翻译成随时间变化的参考ZMP轨迹。ZMP不能像落脚点那样突变，它必须在当前支撑脚的脚底多边形内部，双脚支撑期还得平滑地从一只脚过渡到另一只脚。

常规做法是单脚支撑期把参考ZMP钉在当前支撑脚踝正下方，双脚支撑期让参考ZMP从前支撑脚踝线性移动到后支撑脚踝。双脚支撑时间占步周期的20%到30%比较常见。下面这段代码是简化实现，假设每一步结束点都在前一只脚的位置，也就是双脚并拢式的行走：

% genZMPRef.m % 从落脚点序列生成参考ZMP轨迹 function zmpRef = genZMPRef(steps, stepTime, dsRatio, dt) N = size(steps, 1); totalT = N * stepTime; t = 0:dt:totalT; zmpRef = zeros(length(t), 2); dsTime = dsRatio * stepTime; % 双脚支撑时长 for i = 1:length(t) % 当前落在第几步，钳到最大步数防止越界 stepIdx = floor(t(i) / stepTime) + 1; stepIdx = min(stepIdx, N); timeInStep = mod(t(i), stepTime); if timeInStep < dsTime % 双脚支撑期：ZMP从上一只脚踝线性过渡到当前脚踝 if stepIdx == 1 from = [0, 0]; else from = steps(stepIdx - 1, 1:2); end to = steps(stepIdx, 1:2); alpha = timeInStep / dsTime; zmpRef(i, :) = (1 - alpha) * from + alpha * to; else % 单脚支撑期：ZMP钉在当前支撑脚踝 zmpRef(i, :) = steps(stepIdx, 1:2); end end end

逻辑分两个阶段：时间轴推到每一步的周期里，如果还在双脚支撑窗口内，就从上一脚踝向当前脚踝线性插值；过了这个窗口就钉在当前脚踝上。alpha是过渡进度，从0到1。参数说明：

• stepTime：单步周期，单位秒，典型值0.5到1.0。太快则参考ZMP变化剧烈，质心需要高加加速度去追；太慢又不像走路，更接近原地踏步；
• dsRatio：双脚支撑时间占比，0.2到0.3比较稳。比例太小会让过渡阶段变短，ZMP曲线的斜率变陡；
• dt：采样周期，0.01秒是平衡点，既保证精度又不让序列太长，必须和后面离散化LIPM的dt保持一致。

最容易踩坑的是末段边界：最后一步结束后进入终点站立，时长没有单独建模，参考ZMP会停在最后一脚踝，质心轨迹在末尾会有小幅回落。这个现象第5章专门讲。另外，简化版用线性插值，如果你追求更自然的步态，可以把双脚支撑期的过渡从直线改成三次多项式，质心速度曲线会更顺滑，改动只涉及alpha的计算方式。

3.3 模型离散化与预览增益求解

LIPM方程要变成能写进控制器的形式，先离散化。取状态向量为质心位置、速度、加速度三者的组合，控制输入u是质心加加速度jerk，采样周期dt。前向欧拉离散的结果如下：

% discretizeLIPM.m % 离散化LIPM动力学，返回状态方程矩阵 function [A, B, C] = discretizeLIPM(dt, zc, g) % 状态 x = [px; vx; ax]，控制 u = d(ax)/dt A = [1, dt, dt^2/2; ... 0, 1, dt; ... 0, 0, 1]; B = [dt^3/6; dt^2/2; dt]; % ZMP 输出：p = px - (zc/g) * ax C = [1, 0, -zc / g]; end

A是把当前状态推进一拍的状态转移矩阵，B是控制输入的通道矩阵，C把状态映射到ZMP输出。注意C的写法：ZMP等于质心位置减去加速度乘z_c/g，这正是LIPM方程的变形。离散化格式是预览控制标准推导里的通项写法，0.01秒采样周期下低阶欧拉近似已经够用，没必要引入更复杂的积分器。

拿到A、B、C之后，预览增益要用Riccati迭代离线求解。增广状态里多一维上一拍的控制量，代价矩阵X迭代收敛后拆出反馈增益和预览增益：

% solvePreviewGain.m（主干示意） % 用增广误差状态求预览控制器增益 function [K, F] = solvePreviewGain(A, B, C, Qe, R, Np) n = size(A, 1); % 增广状态：把控制量的增量并入状态，误差盯ZMP输出 Abar = [A, B; zeros(1, n), 1]; Bbar = [B; 1]; Cbar = [C, 0]; % 离散Riccati方程不动点迭代 X = Cbar' * Qe * Cbar; for k = 1:500 S = R + Bbar' * X * Bbar; Kbar = S \ (Bbar' * X * Abar); Xn = Cbar' * Qe * Cbar + ... (Abar - Bbar * Kbar)' * X * (Abar - Bbar * Kbar) + ... Kbar' * R * Kbar; if norm(Xn - X, 'fro') < 1e-12 X = Xn; break; end X = Xn; end % 反馈增益K作用在质心状态上 K = Kbar(1:n); % 预览增益F：沿闭环矩阵H向后滚动 H = Abar - Bbar * Kbar; F = zeros(Np, 1); Xi = (S \ (Bbar' * X))'; for i = 1:Np F(i) = Xi(end); % 参考ZMP对应的分量 Xi = Xi * H; % 向后滚一步 end end

主干示意里我把预览增益的递推简化成沿闭环矩阵H逐拍向后滚，完整实现里末尾还要乘一个投影向量来剥离参考输入通道，这里为了可读性省略。跑仿真时只要K和F曲线与你自己复算的Riccati解一致，就说明递推没写错。参数说明：

• Qe：ZMP误差权重，标量，越大跟踪越紧；
• R：控制增量权重，标量，越大轨迹越平滑；
• Np：预览窗口长度，单位是拍。比如预览1.2秒、dt=0.01时Np=120。

实际工程里预览窗口不是越长越好。窗口越长，提前反应越充足，但离线求解的F序列尾端趋于零，对结果的影响越来越小，反而白白增加内存。常见经验值取1.0到1.6秒的窗口，对应的Np就是100到160拍。

3.4 质心轨迹滚动仿真：simulateWPG

增益求解完成后，质心轨迹的生成就是标准的滚动计算。水平面内的x和y两个方向分别跑一遍预览控制，得到位置、速度、加速度序列。下面给出单方向的实现：

% simulateWPG.m % 预览控制器滚动生成质心轨迹（单方向版本） function [com, vel, acc, zmpOut] = simulateWPG(A, B, C, K, F, pRef, Np) N = length(pRef); n = size(A, 1); x = zeros(n, N); % 状态轨迹：位置/速度/加速度 u = zeros(1, N); % 控制输入：jerk zmpOut = zeros(1, N); for k = 1:N-1 % 取预览窗口内的参考ZMP，末尾越界时补最后一拍的参考值 idx = k:min(k+Np-1, N); p = pRef(idx); if length(p) < Np p = [p, repmat(pRef(end), 1, Np - length(p))]; end % 控制律：反馈 + 预览前馈 u(k) = -K * x(:,k) + sum(F .* p); % 状态推进 x(:, k+1) = A * x(:,k) + B * u(k); end com = x(1, :); vel = x(2, :); acc = x(3, :); zmpOut = C * x; end

控制律一行就装下了：u(k)等于负反馈项加预览前馈项。sum(F .* p)把预览窗口内每个参考ZMP乘以对应增益再求和，权重越大说明那一拍的参考值越影响当前决策。递推完成后从状态矩阵里取出位置、速度、加速度，ZMP输出由C矩阵映射得到。这个仿真有个关键假设：模型已经离线离散化，增益已经离线算好，在线部分只做两个矩阵乘加。这也是预览控制适合嵌入式实时环境的原因——在线计算量远小于MPC这类优化算法。

4. 跑通这份代码：从脚本入口到可视化验证

4.1 运行环境与参数入口

这份代码纯Matlab就能跑，不依赖Simulink，也不依赖Control System Toolbox，因为Riccati迭代是手写的。这意味着任何一台装得起Matlab的机器都能运行，对你二次开发很友好。启动后先打开main_wpg.m，文件顶部的参数区就是入口。

% main_wpg.m 顶部参数区 dt = 0.01; % 采样周期（秒） zc = 0.80; % 质心高度（米） g = 9.81; % 重力加速度（米/秒^2） stepLen = 0.15; % 步长（米） stepWidth = 0.18; % 步宽（米） stepTime = 0.8; % 单步周期（秒） dsRatio = 0.25; % 双脚支撑时间占比 totalSteps = 10; % 总步数 Np = round(1.2 / dt); % 预览窗口1.2秒对应的拍数

这些参数之间有几组关联要留意。Np用round从预览时间换算成拍数，目的是让预览窗口和采样周期解耦，改dt时不用手算Np；dsRatio乘stepTime必须小于stepTime，否则双脚支撑期会覆盖整个步周期；totalSteps设10到20步足够观察稳态规律，太多会让仿真曲线尾部很长，看不出细节。改完参数直接运行，脚本会串起整个链路并弹出两个图形窗口：一个是质心位置、速度、加速度随时间变化的曲线，另一个是机器人前进的平面动画。如果只想看轨迹不想开动画，把main末尾调用animateWalk那行注释掉即可。

4.2 运行结果怎么读：ZMP误差、质心曲线和动画

运行完成后，第一件要确认的事是ZMP曲线的跟踪质量。在仿真输出里对比pRef和zmpOut两条曲线，除了起步和停步阶段会有明显偏差，中间稳定段两条曲线应该几乎重合。如果稳定段误差超过2厘米，先从权重找原因，Qe增大是首选。

质心曲线的读数有讲究。位置曲线整体平滑上升，代表机器人在前进；速度曲线在每个步周期内呈现快慢交替，单脚支撑期快点、双脚支撑期慢点，这是正常形态；加速度曲线是锯齿状的，因为预览控制在持续修正ZMP偏差。最后一到两步加速度会稍微收敛变平，属于边界效应，不用慌。

动画窗口里关注两点：机器人脚下的ZMP点是否始终落在支撑脚底多边形内；脚在单脚支撑期是否保持静止，双脚期才移动。两条都满足，说明步态几何上是稳定的。如果ZMP点擦着支撑多边形边缘走，说明步宽偏小或dsRatio偏小，优先加大步宽。

4.3 调参数让机器人走得更自然

跑通默认参数后，按下面的顺序微调效果最好。先动步长和步周期：stepLen = 0.2、stepTime = 0.6，观察速度曲线是否更接近自然行走的钟摆感。再调双脚支撑比例：dsRatio从0.25往0.15降，ZMP过渡会更利落，但质心速度波动变大；往0.3升则更稳更慢。最后调权重：跟踪不准优先增大Qe，轨迹过冲优先增大R。

我个人的调试顺序是从时间参数改起，因为ZMP参考的变化频率主要由步周期决定；权重是用来收尾的，不是用来解决周期性问题的。如果改完发现质心位置曲线在步与步之间出现高频抖动，基本可以断定是R太小，把R从1e-4升到1e-3再看。这一系列调整的核心还是回到第2章那个公式：ZMP决定质心怎么走，预览控制决定ZMP追得多紧。调参时脑子里要有这条因果链，不要同时动两三个变量，出了问题都不知道是谁的锅。

提示：改参数前先备份一份原始的main_wpg.m。我当年就是改完没存，想对比旧参数又改不回去，翻了好久的车。

5. 常见问题与避坑指南

5.1 现象：Riccati迭代不收敛，增益输出NaN

把R设成0或非常小的值，比如1e-12，增益求解直接返回NaN。原因是控制增量的惩罚项太小，最优化问题接近病态，矩阵S求逆时接近奇异。解决的第一步，也是血泪经验：R不要小于1e-6，常规范围在1e-4到1e-2之间。第二步是检查增广矩阵，Bbar最后一行是否为1，如果你自己扩展状态时把Bbar拼错，Riccati迭代必散。遇到NaN先别怀疑公式，先确认每一步的矩阵维度，用disp(size(Abar))、disp(size(Bbar))打出来看一遍，维度对不上基本就是拼错。

5.2 现象：ZMP参考在双脚支撑期跳变，速度曲线出现尖角

genZMPRef里双脚支撑期的线性插值不应该有突变，但如果改过dsRatio，发现ZMP曲线在双脚期进入单脚期的瞬间拐出一个尖角，多半是过渡起点取错了。检查代码里from变量是不是取了steps(stepIdx-1)而没判断第一步的边界。第一步时from取[0,0]是把机器人的初始位置当作虚拟前脚，如果初始质心位置不在原点，这个假设就不成立。解决：把初始质心位置作为参数传入genZMPRef，用真实值替代原点。严格做法是让ZMP在两只脚踝之间按时间比例插值，还要考虑支撑多边形重心，但线性插值对预览控制来说够用。

5.3 现象：质心轨迹在最后一两步掉下去

末端效应，几乎每个跑预览控制的人都会遇到。原因是参考ZMP在最后一步之后没有继续给，预览控制器的前馈通道在窗口尾部补的是最后一拍参考值，相当于按静止状态收尾。可机器人实际停下来时还要把重心移到两脚之间，参考ZMP应该有个收拢过程。解决的常见做法是在最后补一段站立参考：把最后一步的ZMP参考值保持0.3秒到0.5秒，再线性过渡到两脚中心，让预览控制器有时间把质心速度降下来。代码里就是在zmpRef末尾多append一段固定值或斜坡，改动量很小但效果立竿见影。

5.4 现象：预览窗口调大，结果反而变慢变软

很多人以为预览窗口越长越好，实际行为是：窗口从0.5秒增到1.5秒时明显改善，继续增到3秒，轨迹几乎不再变化。原因在于预览增益F序列是衰减的，窗口后半部的增益已经趋近于零，多出来的参考值乘上接近0的系数，等于白算。这个现象不是bug，是指标函数里的权重决定了有效展望距离。想真正延长有效预览，应该增加Qe相对R的比例，而不是单纯拉长Np。这一点很反直觉，当年我在这上面耗了快一周，最后对着F序列曲线才想明白。

5.5 现象：动画里脚在原地打滑

脚打滑基本可以锁定是脚序列与ZMP参考没有对齐。典型错误是把落脚点时间下标和ZMP时间下标错开一拍：脚序列在第k步落地，ZMP参考却等到第k+1步才把支撑脚切过去。检查genZMPRef里stepIdx = floor(t(i)/stepTime)+1是否和你的步态时钟起点一致。如果步态时钟从0开始、脚序列第一步也在0时刻，两者对齐很简单；如果你在别处给时间轴加了偏移量，问题就来了。解决：在main里把时间轴起点统一打出来，人工对比第n步的落地时刻和ZMP参考切脚时刻，偏差必须小于一个dt。

6. 二次开发：把预览控制嵌进你的机器人框架

6.1 抽接口：把脚本拆成能被调用的函数

原代码的main是顺序脚本，二次开发第一步是把它改造成函数接口。配置塞进一个结构体cfg，所有函数不再依赖任何全局变量，这样才能被测试脚本和机器人主程序分别调用。

% buildWalkingPattern.m % 输入步态参数，输出质心轨迹与脚序列，供上层控制器调用 function [comTraj, zmpRef, footSeq] = buildWalkingPattern(cfg) [steps] = genFootsteps(cfg.stepLen, cfg.stepWidth, cfg.totalSteps); [zmpRef] = genZMPRef(steps, cfg.stepTime, cfg.dsRatio, cfg.dt); [A, B, C] = discretizeLIPM(cfg.dt, cfg.zc, cfg.g); [K, F] = solvePreviewGain(A, B, C, cfg.Qe, cfg.R, cfg.Np); [comTraj, ~, ~, ~] = simulateWPG(A, B, C, K, F, zmpRef(:,1), cfg.Np); % y方向的流程与x方向相同，这里省略，实际代码里两份串行跑 footSeq = steps; end

6.2 从直线走到转弯：改脚序列生成器

改造成转弯步态时，只需动genFootsteps。转弯的本质是每一步额外旋转一个角度theta，落脚点的坐标系跟着转：

% 转弯步态核心：旋转矩阵作用于当前步的偏移 R = [cos(theta), -sin(theta); sin(theta), cos(theta)]; steps(i, 1:2) = steps(i, 1:2) * R;

注意ZMP参考生成和质心轨迹要同步切换坐标系，否则脚序列转了、ZMP参考还在直线上，两套坐标系打架，动画里脚必滑。

6.3 验证改造的正确性

改完任何一步，先跑同参数下的旧接口和新接口，把两条质心位置曲线做差，差值应在浮点误差范围内。然后设一组极端参数，比如大步长、快周期、小R，跑仿真看ZMP是否仍在支撑多边形内，这是比曲线更硬性的安全指标。验证时特别要盯着最后一步的设计——在线机器人跑完一序列步态后要能站稳，才敢接到底层控制器里去。从那以后我每次改步态生成代码都强制走一遍同参数对比、极端参数压测、末端站立检查这三条验证，习惯了。希望帮到你。

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