六足机器人这个坑，我是从一台旧笔记本加一份MATLAB授权开始的。当时手上没有舵机、没有结构件、连一根杜邦线都没有，却特别想弄清楚一个看起来很简单的问题：六条腿到底按什么顺序抬起来，才能走得又稳又快？这个问题的答案就是步态，而想验证步态方案，最便宜、最安全、也最灵活的方式，就是在MATLAB里做步态仿真。
这篇文章把我最近从零搭起来的一套六足机器人步态仿真框架完整拆给你看，覆盖交替三角步态和波动步态两种典型行走方式。从运动学建模、足端轨迹规划、步态时序生成，到三维可视化、稳定性指标计算，每一步都给出可直接复制的代码和参数说明。适合三类人：准备做六足机器人但还没动手的，已经写了点程序但走起来像“醉汉过弯”的，以及想用MATLAB做机器人运动学仿真练手的学生和工程师。我能保证的是，按这个教程走一遍，你至少能拥有一套干净、能扩展、随时可以换步态的仿真骨架。
1. 整体设计思路与步态原理拆解
动手写代码之前，先想清楚一个问题：步态仿真到底在仿真什么？
很多新手以为步态仿真就是让一个3D模型在屏幕上动起来，于是花大量时间去建模、贴材质，最后腿是动了，但完全说不清楚它为什么这么动。实际上，步态仿真的核心研究对象只有一个：支撑相和摆动相的时间分配。一条腿接触地面、支撑机体的阶段叫支撑相，离开地面、向前迈进的阶段叫摆动相。步态就是给六条腿各自分配“什么时候撑、什么时候迈、迈多远、抬多高”。搞懂了这个分配逻辑，剩下的运动学计算和可视化都是工具而已。
1.1 六足机器人的腿部结构与自由度
本文使用最常见的三关节腿模型，每条腿3个自由度，六条腿共18个自由度。三个关节分别是：
- 髋关节侧摆（yaw）：绕机身垂直轴旋转，控制整条腿向机身两侧张开的角度。
- 髋关节前摆（pitch）：绕机身横向轴旋转，控制大腿前后摆动。
- 膝关节（pitch）：控制小腿相对于大腿的弯曲，负责抬脚和伸腿。
简化模型的腿部连杆参数我用以下一组，方便复现：
- 机身半长 0.25 m，半宽 0.16 m。
- 髋关节到地面目标高度约 0.12 m（机身悬浮高度）。
- 大腿长度 L1 = 0.10 m。
- 小腿长度 L2 = 0.15 m。
- 髋关节侧摆安装点到腿平面的偏距 0.05 m，建模时可以并入初始yaw角。
腿的编号规则也提前约定清楚，后面所有代码都基于这套编号：
| 编号 | 位置 | 分组（三角步态） |
|---|---|---|
| 1 | 左前 | A组 |
| 2 | 左中 | B组 |
| 3 | 左后 | A组 |
| 4 | 右前 | B组 |
| 5 | 右中 | A组 |
| 6 | 右后 | B组 |
这个编号方式是从机身俯视方向定义的：左侧是机器人前进方向的左手边。先把编号定死，代码才不会写到一半把自己绕晕。
1.2 交替三角步态：靠三腿支撑走天下的高速步态
交替三角步态是六足机器人最常见的步态，昆虫里蟑螂、蚂蚁快速跑动时用的就是类似策略。它的核心思想是：把六条腿分成两组，每组三条腿，任意时刻一组完全抬起向前摆动，另一组完全着地支撑身体。两组交替工作，所以叫“交替三角”。
为什么叫“三角”？因为三条支撑腿的落点在地面上会构成一个三角形，机身重心只要落在这个三角形内部，机器人就不会翻倒。A组由左前腿（1）、左后腿（3）、右中腿（5）组成，B组由右前腿（4）、右中腿（6）、左中腿（2）组成。你会发现每组的三条腿在空间上天然呈三角形分布，这是经过设计的：一组前腿、一组后腿、再加对侧的中腿，支撑多边形面积最大。
三角步态的占空比是0.5，也就是任意时刻有一半的腿在支撑、一半在摆动。它的优点是行进速度快，同时运动对称性也好，机身波动小。缺点同样明显：支撑腿只有三根，支撑三角形面积相对有限，一旦步幅拉大或者地形不平，重心很容易跑到三角形外面，稳定性变差。所以在平整地面上追求速度时，选三角步态。
1.3 波动步态：五腿支撑的稳健行走
波动步态是另一种极端策略：六条腿按照从后向前的顺序依次抬起，任意时刻最多只有一条腿处于摆动相，其余五条腿支撑地面。因为支撑腿多，支撑多边形面积非常大，重心几乎不可能跑出支撑区域，所以这种步态极其稳定。
代价是速度。五条腿支撑意味着摆动腿数量被压缩到一条，每一步的行程有限，整机前进速度大约只有三角步态的一半甚至更低。另一个特点是它看起来像水波一样从尾部向头部传递——“波动”这个叫法很形象。实际昆虫慢速爬行时也采用类似策略，慢但稳。
两种步态的对比可以浓缩成一张表：
| 指标 | 交替三角步态 | 波动步态 |
|---|---|---|
| 同时摆动腿数 | 3条 | 1条 |
| 支撑腿数 | 3条 | 5条 |
| 稳定性 | 中 | 高 |
| 速度 | 快 | 慢 |
| 支撑多边形面积 | 小 | 大 |
| 典型场景 | 平地快速移动 | 崎岖地形、高负载 |
我不建议一上来就纠结“哪种步态更好”，因为这不是选择题，是场景适配题。仿真里把两种都实现了，后面调参数时你自然能感受到它们的性格差异。
1.4 为什么用MATLAB而不是C++或Python
很多人质疑：机器人仿真不是应该用ROS、Gazebo、PyBullet吗？为什么还在用MATLAB老古董？
我的观点是：**工具要看阶段。**在算法验证和原理学习阶段，MATLAB的优势非常突出。矩阵和向量运算不用写循环，plot3、patch几行代码就能出三维图，脚本热重载调试极快，内置数学函数齐全，逆运动学里那些atan2、acos、cosine定理根本不需要自己造轮子。Python的matplotlib虽然也能画3D，但交互流畅度和MATLAB的图形句柄系统还是差一口气。
另外一个实际原因：你要走的腿只有18个关节自由度，步态规划本质上是一个时序问题，用矩阵表达非常自然。MATLAB的向量化编程风格正好契合这个场景。至于性能，MATLAB在纯运动学仿真这个量级完全够用，我的经验是，在普通笔记本上，一个步态周期实时仿真加可视化，帧率能稳定在30帧以上，根本不卡。
2. 从零搭建仿真环境与运动学模型
2.1 准备MATLAB环境与脚本组织
本文代码基于MATLAB R2022b左右编写，但只用了最基础的函数，理论上R2018b之后的版本都能跑。不需要额外安装Robotics System Toolbox，虽然装了会方便一些，但我们的逆运动学几行手写代码就能解决。
你需要确保手头有可用的MATLAB。如果还停留在安装阶段，这里只说一点：优先使用学校、单位购买的正版授权，或者MathWorks官方的试用版，别在下载阶段浪费太多精力。装好后打开编辑器，提前规划好几个脚本文件，我习惯这样组织：
hexapod_sim/ ├── main_tripod.m % 交替三角步态主程序 ├── main_wave.m % 波动步态主程序 ├── inverse_kinematics.m % 单腿逆运动学 ├── foot_trajectory.m % 足端摆动/支撑轨迹 ├── gait_phase.m % 步态时序生成 ├── draw_hexapod.m % 可视化 └── stability_margin.m % 稳定性裕度计算每个函数单独一个文件是MATLAB的惯例，也可以把所有函数放在主脚本末尾。新手建议用单独文件，报错时定位更快。
2.2 建立运动学模型：从机身到足端
六足机器人运动学分成两层：正运动学和逆运动学。正运动学是给关节角求足端位置，逆运动学是给期望足端位置求关节角。步态仿真里我们主要用逆运动学，因为步态规划器输出的是“足端应该走到哪”，我们需要把它换算成关节角度去驱动模型。
先明确坐标系关系。世界坐标系W固定在地面，机身坐标系B随机器人在W中平移和旋转。第i条腿的髋关节安装位置在机体系里是已知的固定值，用hip_pos_body(i, :)表示。足端在世界系下的目标是步态规划器给的，换算到机体系下，再换算到髋关节系，最后交给逆运动学。
逆运动学的推导我用二维展开来理解。给定足端相对髋关节的三维坐标 [X, Y, Z]，先把问题平移到XOZ平面：
- 先算髋关节侧摆角 q1 = atan2(Y, X)，这一步把大腿和小腿所在的平面转到一个平面内。
- 转换到这个髋部平面坐标系后，足端到髋关节的水平距离 r = sqrt(X^2 + Y^2)，垂直距离 s = Z。
- 在大腿-小腿-足端构成的三角形中，大腿和小腿之间的夹角由余弦定理决定： D = (r^2 + s^2 - L1^2 - L2^2) / (2 * L1 * L2) q3 = -acos(clamp(D, -1, 1)) 其中q3为膝关节角，负号表示小腿向后弯曲。
- 最后求髋关节前摆角q2。先用atan2(s, r)得到大腿根部到足端的连线与水平面夹角，再用余弦定理得到大腿与该连线的夹角alpha，则q2 = atan2(s, r) - alpha。
对应的MATLAB函数如下：
function q = inverse_kinematics(p_hip_to_foot, L1, L2) % p_hip_to_foot: 足端在髋关节系下的坐标 [X; Y; Z] % 输出 q = [q1; q2; q3] 分别对应 髋yaw、髋pitch、膝关节pitch X = p_hip_to_foot(1); Y = p_hip_to_foot(2); Z = p_hip_to_foot(3); q1 = atan2(Y, X); r = sqrt(X^2 + Y^2); s = Z; D = (r^2 + s^2 - L1^2 - L2^2) / (2 * L1 * L2); D = max(-1, min(1, D)); q3 = -acos(D); alpha = acos((L1^2 + r^2 + s^2 - L2^2) / (2 * L1 * sqrt(r^2 + s^2))); q2 = atan2(s, r) - alpha; q = [q1; q2; q3]; end这个函数是整个仿真的地基。测试它非常简单，给一组在可达空间内的坐标，算出来的关节角再代回正运动学，看足端是否回到原位。初次写完后一定要做这个往返验证，可以省下后面排查的大量时间。
2.3 足端轨迹规划：支撑相与摆动相必须分开处理
逆运动学拿到的是目标位置，目标位置来自足端轨迹规划。足端轨迹分为支撑相轨迹和摆动相轨迹，两者逻辑完全不同。
摆动相是腿离开地面向前迈的过程。它的起点是当前落地点，终点是前进一个步幅后的落地点，中间要抬到一定高度避开障碍。最常用的轨迹是摆线轨迹（cycloid），因为它的速度曲线在起点和终点处均为零，加速度连续，协调性好。设步幅S、抬腿高度H、摆动相时长T_sw，摆动进度u从0到1，则足端相对摆动起点的偏移为：
- 前进方向：x = S * (u - sin(2πu) / (2π))
- 抬腿方向：z = H * (1 - cos(2πu)) / 2
这个轨迹的特点是：腿在中间时刻抬得最高，落地瞬间速度和加速度为零，不会砸地。
支撑相则相反。支撑腿落在地面上时，相对世界坐标系的位置应当保持不变，但机身一直在向前移动，所以支撑腿的足端相对机身坐标必须主动向后移动，速度与机身前进速度大小相等、方向相反。这一步是新手最容易漏的，一旦漏掉，你的机器人就会呈现出“原地踏步却推着地面走”的诡异效果，或者说支撑腿在地上打滑。
所以支撑相的足端轨迹是相对机身的匀速直线运动，从摆动落点向后滑动一个步幅的距离。就这一段补充：支撑腿不是真的在地面摩擦滑动，而是相对世界坐标静止，看起来像是向后“抹”过去，实际接触点没变。
3. 交替三角步态从零实现
3.1 相位模型与支撑/摆动状态的生成
交替三角步态的时序逻辑极其简单：定义相位progress = mod(t, T) / T，T为一个完整步态周期。在一个周期内，前半个周期让A组摆动、B组支撑，后半个周期反过来。
function [swing_mask, progress] = tripod_phase(t, T) % 返回每条腿是否处于摆动相，以及当前周期进度 progress = mod(t, T) / T; swing_mask = zeros(6, 1); group_A = [1; 3; 5]; group_B = [2; 4; 6]; if progress < 0.5 swing_mask(group_A) = 1; else swing_mask(group_B) = 1; end end这份代码的逻辑放在主循环里逐帧调用。基于它再生成每条腿当前的目标足端位置：running_mask为1的腿走摆动轨迹，为0的腿走支撑轨迹。
值得提醒的是，摆动相和支撑相的起始点需要跨周期连续。比如A组第一个周期摆动落点位置是P_start，下一个周期摆动落点就变成了P_start + S。如果直接用世界坐标存储每条腿的“落地点”，后面的位置更新会简单很多：每次摆动结束时，把该腿的落地点向前增加S。
3.2 步幅、抬腿高度与周期的参数计算
参数不能乱拍。步幅S主要受腿部工作空间限制。我的模型里大腿0.10m、小腿0.15m，加上髋关节侧摆角约25度，单腿足端在水平面上的可达半径大约0.19m。实际走直线时，步幅S取0.08m比较安全，留出余量。
抬腿高度H与步幅挂钩。步幅越大，摆动过程中腿部轨迹弧度越大，越需要更高空间避免擦地。S=0.08m时，H=0.03~0.04m足够，太高的抬腿会浪费时间和能量，还会增加膝关节角变化幅度。
周期T则由期望前进速度和步幅共同决定。机身前进速度近似等于 S / T。举例：S=0.08m，T=1.0s时，速度约0.08m/s，适合做原理验证；如果你希望走到0.2m/s，就把T缩到0.4s。这里要注意，摆动相和支撑相的时间分配是固定的（各占0.5T），所以缩T相当于同时加快迈腿和支撑滑动速度。三角步态的摆动相时长也不需要单独调，直接是0.5T。这些参数会直接进入轨迹函数：
function p = foot_target(leg_idx, swing_mask, progress, params, leg_ground) % 根据步态状态和足端落地点计算目标位置 S = params.S; H = params.H; T_sw = params.T/2; if swing_mask(leg_idx) == 1 u = progress / 0.5; % 摆动进度0到1 dx = S * (u - sin(2*pi*u)/(2*pi)); dz = H * (1 - cos(2*pi*u)) / 2; p = leg_ground(:, leg_idx) + [dx; 0; dz]; else % 支撑相：相对机身均匀后移 u = (progress - 0.5) / 0.5; p = leg_ground(:, leg_idx) + [S * u; 0; 0]; end end支撑相这段代码里的leg_ground保存的是该腿本周期摆动落地时的世界坐标，这一帧的位置是抬起来之前的地面点。这样写简单直观，但要注意这里p是“云端”的足端位置，下支撑阶段的腿其实是固定不动的，它相对世界坐标不动，相对机身向后“滑动”。
3.3 机身位移与姿态保持
机身位移是步态仿真里另一个容易出错的地方。最省事的做法是让机身始终沿X轴匀速前进，速度v = S / T。先给定机身在世界系的位姿，再把足端目标位置换算到机体系。
这里有一个关键细节：足端目标位置必须用世界坐标统一计算，再换算到机体系，不能反过来用机体系坐标直接叠加。否则支撑腿会随机身移动，导致地面打滑。
机身姿态我建议在整个直线行走过程中保持水平零姿态，也就是roll=pitch=yaw=0。很多新手一上来就想着折腾姿态补偿，我踩过坑之后特别想说：先走直线，姿态全零，确认步态正确后再考虑转弯和姿态稳定。转弯相当于给机身前进方向加一个角速度，需要重算每条腿的世界系落点，那是另一个工作量。
主循环里这样写：
body_x = v * t; R_BW = eye(3); % 机身水平，旋转矩阵为单位阵 for i = 1:6 p_world = foot_target(i, swing_mask, progress, params, leg_ground); p_body = R_BW' * (p_world - body_pos); p_hip = p_body - hip_pos_body(i, :)'; q(:, i) = inverse_kinematics(p_hip, L1, L2); end这样算出来的关节角喂给可视化模块，一条走得干净的三角步态就出来了。
4. 波动步态从零实现
4.1 波动步态的相位偏移机制
波动步态和三角步态最大的区别在于相位分配策略。三角步态是“两班倒”，一半腿干活一半腿休息，相位要么0要么0.5。波动步态是“轮流值班”，六条腿按固定顺序错开相位，每条腿的摆动窗口宽度很窄，避免同时有两条腿离地。
相位分配的数学表达很简洁：第i条腿的相位偏移phi_i = (i-1) * 2π/6，也就是60度一个间隔。我把编号顺序定为从后往前：腿3（左后）先动，然后腿2（左中）、腿1（左前）、腿6（右后）、腿5（右中）、腿4（右前）。这样摆动像波浪一样从尾部传向头部，与昆虫实际慢速行走模式一致。
function swing_mask = wave_phase(t, T, swing_duty) % swing_duty: 摆动窗口宽度，例如0.2表示摆动占周期20% omega = 2*pi / T; phase = mod(omega * t, 2*pi); swing_mask = zeros(6, 1); order = [3; 2; 1; 6; 5; 4]; for k = 1:6 leg = order(k); leg_phase = mod(phase - (k-1) * (2*pi/6), 2*pi); if leg_phase < (swing_duty * 2*pi) swing_mask(leg) = 1; end end endswing_duty是关键参数。理论上六条腿相位间隔是60度，摆动窗口如果取50度，就能保证任何时刻最多一条腿摆动。如果窗口大于60度，必然出现两条腿同时摆动的重叠时刻，波动的稳定优势就打折扣。
4.2 波动步态的参数调节与速度关系
波动步态需要调节的参数比三角步态多一个摆动窗口swing_duty。我先给出一个比较稳妥的初始值组合：
- 步幅S = 0.08m。
- 抬腿高度H = 0.04m。
- 周期T = 2.0s。
- swing_duty = 0.2（即摆动窗口占周期20%，换算成角度是72度）。
等一下，这里要解释一下。虽然六条腿相位间隔60度，窗口72度看起来会重叠，但要注意实际摆动进度分配。更合理的方式是让摆动窗口严格小于相位间隔，比如20%周期对应72度，确实大于60度，所以会出现重叠窗口。
正确的做法是swing_duty < 1/6 ≈ 0.167。我推荐取0.15，换算成时间是0.3s（T=2s时）。这样任何时刻只有一条腿在摆动，最多在切换瞬间出现一条腿结束一条腿开始的临界状态，无重叠。
T=2s时，摆动相0.3s，支撑相1.7s，占空比5/6，符合波动步态“绝大多数时间五腿支撑”的特征。速度v = S/T = 0.04m/s，比三角步态慢一半。如果你嫌慢，可以同时缩短T和扩大S，但别动swing_duty这个比例。
我把三角步态和波动步态的初始参数表列在一起，方便对照：
| 参数 | 三角步态 | 波动步态 |
|---|---|---|
| 步幅S | 0.08m | 0.08m |
| 抬腿高度H | 0.04m | 0.04m |
| 周期T | 1.0s | 2.0s |
| 摆动窗口 | 0.5T | 0.15T |
| 同时摆动腿数 | 3条 | 1条 |
| 理论速度 | 0.08m/s | 0.04m/s |
4.3 两种步态的切换策略
做仿真时最容易忽略的就是步态切换过程。假如你从三角步态直接切到波动步态，从时间到相位状态全都变了，每条腿的目标位置会跳变，仿真画面里机器人会像触电一样抖一下。
最稳妥的切换方式是“先停再切”：先把所有腿设为支撑相，保持机身静止几个仿真步，再启动新步态。停下再走看似浪费了几秒，但省去大量调切换逻辑的时间和踩坑成本。这也是我在实际项目中常用的做法。程序里实现特别简单——把主循环状态机的状态从run_tripod切到stop，再切到run_wave即可，期间每条腿的落地点不变。
更高阶的平滑切换可以在步态切换时，让每条腿的相位变量和落地点做插值过渡。核心是保证足端位置连续。篇幅原因这里不展开，等你把基础版本跑通，自然知道在哪里加。
5. 仿真可视化与结果分析
5.1 实时三维可视化的实现要点
MATLAB的3D可视化用plot3和patch两个函数就能搭出像样的效果。机身画一个矩形面片，每条腿画三条连线（髋关节-大腿-小腿-足端），足端位置用加粗圆点标出来。
实时刷新时，千万不要用clf清除图形重画，那样会闪瞎眼而且极慢。正确做法是用set函数更新现有图形对象的XData、YData、ZData：
figure('Color', 'w'); hold on; axis equal; grid on; view(3); body_patch = patch('XData', body_xs, 'YData', body_ys, 'ZData', body_zs, ... 'FaceColor', [0.3 0.6 0.9], 'EdgeColor', 'k'); leg_lines = gobjects(6, 1); foot_dots = gobjects(6, 1); for i = 1:6 leg_lines(i) = plot3(0, 0, 0, 'k-', 'LineWidth', 2); foot_dots(i) = plot3(0, 0, 0, 'ro', 'MarkerSize', 6, 'MarkerFaceColor', 'r'); end for k = 1:num_steps % ... 计算 q, body_pos ... % 正运动学求各关节世界坐标 % set(body_patch, 'XData', ..., ...) % set(leg_lines(i), 'XData', ..., 'YData', ..., 'ZData', ...) % set(foot_dots(i), ...) drawnow limitrate; enddrawnow limitrate是MATLAB在实时动画里的一个隐藏技巧，它限制每秒刷新率不超过20帧左右，但对仿真来说足够，而且CPU占用比普通drawnow低得多。在仿真步数大的时候能救你一命。
5.2 重心稳定性曲线：眼见为实
可视化只是让你“看起来稳”，实际稳不稳要看数值指标。最经典的稳定性判据是重心投影点是否落在由支撑足端构成的凸多边形内部。MATLAB里可以用convhull求凸包，再用polyshape判断点是否在多边形内部。
稳定余量用重心到支撑多边形各边的最短距离来衡量，我把它封装成函数：
function margin = stability_margin(support_foot_positions, cog_xy) % support_foot_positions: 支撑腿足端地面坐标 Nx2 % cog_xy: 重心水平投影坐标 1x2 poly = polyshape(support_foot_positions(:, 1), support_foot_positions(:, 2)); [vertices, ~] = boundary(poly); nv = size(vertices, 1); dists = zeros(nv, 1); for i = 1:nv j = mod(i, nv) + 1; dists(i) = abs(det([vertices(j,:)-vertices(i,:); cog_xy-vertices(i,:)])) ... / norm(vertices(j,:) - vertices(i,:)); end margin = min(dists); end数值为正且越大越稳，接近0说明重心已经贴近支撑三角形边缘，负值意味着摔倒。用三角步态跑一圈再切波动步态跑一圈，你可以非常直观地看到：三角步态每一帧稳定余量都会周期性波动，最小余量明显偏小；波动步态的余量则长期保持在高位。这就是两种步态稳定性的数字化体现，比嘴上说“波动更稳”有说服力得多。
5.3 可视化结果能告诉我们什么
完成一段仿真后，我还建议做一个动作：把某一条腿的足端轨迹画出来，看它是否形成了规则的“拱形”闭合曲线。摆动相对应的是一段上凸的弧线，支撑相对应的是一段贴近地面、向后滑的直线。如果支撑相轨迹出现了前后抖动，或者摆动相轨迹不圆滑，说明参数不匹配或者相位衔接有问题。
另一个值得画的是机身的垂直位移曲线。理论上身体应该保持恒定高度，但实际上由于支撑-摆动切换时髋关节负载变化，机身高度会有微小波动。波动幅度越小，说明步态越平稳。这个指标也能用来比较三角步态与波动步态的行走品质。
6. 常见问题与排查技巧实录
6.1 逆运动学出NaN或关节角异常
如果在仿真过程中inverse_kinematics返回NaN，或者关节角突然跳到几万度，原因几乎只有一个：目标足端位置跑到了机械臂可达空间之外。
排查方法分两步。第一步，检查你的步幅S是否过大。L1+L2=0.25m，但关节有角度限制，实际可达半径小于这个值。我把S从0.08m调大到0.15m，立刻出现NaN，之后无论怎么调时序都白搭。第二步，在逆运动学函数内部加保护性钳位。前面函数里D = max(-1, min(1, D))就是对acos参数的保护，这一步不只是防止报错，还能让你把问题暴露在轨迹规划层面而不是数学层面。
我强烈建议在开始完整步态仿真之前，先写一个单腿测试脚本：给一个目标点，算角度，画模型，手动改变目标点位置，确认整条腿的关节运动和预期一致。这半个小时的时间投资能省下后面数小时的抓狂排查。
6.2 支撑腿打滑或机器人身体漂移
出现支撑腿打滑，从可视化上看就是支撑腿的足端在地面上留下了拖痕，而不是像被“钉”在地上。根本原因通常是坐标系混杂。
常见错误是：在机体系里直接计算支撑腿目标位置，却叠加了随时间变化的机身位移。正确做法我前面强调过：先在统一的世界系里规划每帧的足端目标位置，再转换到机体系，最后转换到髋关节系。三者顺序不能乱。
另一个隐藏坑：支撑相运动方向做反了。比如支撑腿相对机身移动的方向跟机身前进方向同向，相当于支撑腿也在“推”机身向前，结果就是足端往前滑、机身剧烈甩动。检查轨迹时注意：支撑腿相对机身的速度方向应当是运动方向的负方向。
6.3 机器人步态抖动、震荡与发散
抖动出现在摆动相和支撑相切换的瞬间，多半是相位连续性出了问题。比如摆动相进度u从0.99直接跳到支撑相的0.0，足端位置巨大跳变。检查你用的mod取模运算有没有跨周期，以及落地点更新是否在正确的时刻执行。
如果是持续震荡、机身上下抖动不停，更可能是算法时间步长太大，导致相位变化不连续。我的经验是仿真步长取周期T的1/1000以下，三角步态T=1s时就取0.001s。更大的步长虽然跑得快，但关节角突变、逆解失败、可视化卡顿各种问题都会冒出来。别省这点计算量，先把时间步长锁死在1ms。
最后一个原因是正运动学和逆运动学的腿方向定义不一致。比如正运动学里膝关节正向是顺时针，逆运动学算出来是逆时针，那么算出的关节角代回去足端位置就完全错位。这种错误用单腿往返验证几十秒就能查出来，所以我再次强调：所有运动学代码写完先做往返验证，再做全腿步态仿真。
我把几个高频问题整理成速查表，方便你贴在屏幕旁边：
| 问题现象 | 可能原因 | 解决动作 |
|---|---|---|
| 角度NaN | 目标超出工作空间 | 缩步幅、加钳位保护 |
| 足端位置错乱 | 坐标系混用 | 统一在世界系计算再转换 |
| 支撑腿打滑 | 支撑相速度方向搞反 | 检查相对机身的支撑运动方向 |
| 切换瞬间抖动 | 相位不连续 | 先停再切，或做相位插值 |
| 机身上下震荡 | 时间步长太大 | 缩小步长到T/1000以下 |
| 速度远低于预期 | 周期T太长 | 缩短T或增大S |
6.4 一套通用的参数调优流程
如果你不想看那么多细节，就想快速得到一个能走的仿真，我总结了一套调参流程，照着做基本不会翻车：
- 先固定机身高度、腿长、机身尺寸，不许中途改模型。
- 单腿测试逆解，确认可达空间边界。
- 跑三角步态，S从0.04m起，T设1s，先让它走起来，别追求速度。
- 逐步加大S到0.08m，观察稳定余量和抖动。
- 加跑波动步态，swing_duty从0.1起，比较两种步态的稳定余量曲线。
- 最后优化T，把你关心的速度指标代入S/T计算，以整秒为单位调整T。
换成更先进的步态优化算法（比如遗传算法调步态参数）之前，这套流程已经能覆盖绝大多数入门项目。
我个人在实际操作中的体会是：六足步态仿真最大的门槛不在代码，而在“能不能清楚描述每条腿在每个时刻应该在什么位置”。那个支撑相和摆动相的时间关系图、相位图，往纸上一画，代码就是照抄逻辑。还有一个很实用的建议——把参数和腿的位置数据全部保存下来，跑完仿真后用plot一条一条检查，而不是只看动画。动画看的是热闹，曲线看的是门道。最后多嘴一句：如果你后面真要把这套步态搬上实体机器人，记得在仿真里提前把电机角度限位、速度饱和、支撑力分配加进去，否则从仿真到实物的距离可能比你想象的长。这次先把手上的MATLAB仿真跑明白，就是一个不错的起点。
