简介：双足机器人设计.doc 是一份面向嵌入式机器人开发者与运动控制学习者的步伐源代码解析手册，围绕北京汉库科技 HGR-3HV-C 型号展开，解决双足行走时关节协同与稳定性控制的入门及调参问题。文档从初始站立子程序切入，逐段拆解双腿下蹲、起立、侧身、单腿抬起等动作的变速子程序，按第1族、第2族分阶段说明电机速度、关节角度与重心变化之间的配合关系，并涉及姿态检测、传感器融合、动力学模型与控制算法等配套模块，全篇约101页。整包包含1个doc文件，约9.07MB，无需解压多个子目录即可直接查阅，适合对照源码逐节阅读，目前已有477人学习下载。读者可据此理解步态生成的分段思路，掌握各子程序的参数含义与调试顺序，为定制行走模式、改善动态性能与能效提供可复用的参考框架。

1. 一份双足机器人设计文档里真正决定成败的三件事：腿型、步态、关节带宽

把一个双足机器人从图纸推到地上站稳，最先出问题的往往不是会不会写代码，而是三组数字没有对齐：连杆长度与质心位置、步长步频与质心高度、关节刚度与闭环带宽。这三组数字对不上，控制器再花哨，也会在第一次抬腿时抖成一团。下面按我自己做过的顺序拆：先定腿型，再算步态，然后调关节，最后列一份跑现场时救过命的排查清单。适合正在写设计文档、准备搭一台 10～25 kg 级双足平台的人看，机械和控制都要碰一点。

2. 腿型与自由度选型：12 个关节怎么分配到髋、膝、踝

设计文档最容易出现的问题是花几十页描述外观，却只字不提自由度分配。等你把结构图发给加工厂，才发现踝关节选了个只有 ±15° 行程的舵机，连站立时的重心微调都做不到。我一般会在动手画零件前，先把自由度配置写成一张对比表，把每一档的代价摊开。

2.1 髋 3 + 膝 1 + 踝 2：每条腿 6 个自由度怎么取舍

髋部三个轴（roll / pitch / yaw）负责侧向平衡与转向，膝一个 pitch 决定腿长，踝两个轴负责把零力矩点压在支撑多边形里。这是通用解，但不是唯一解，取舍关键在“你现在要验证什么”。

我的建议是原型阶段直接上 12 自由度平足。理由很实际：调试期你需要机器人能“断电不倒”，能在双支撑状态下慢慢改参数，而点足一旦掉电就是趴地上。踝 roll 的行程给到 ±20° 就够，别追求大角度，行程越大回差越难控。

选完自由度要立刻算一个数：单腿在支撑相需要扛多少力矩。以 18 kg 整机为例，单腿支撑时髋 pitch 峰值约 35 N·m、膝约 30 N·m、踝 pitch 约 22 N·m。这三个数直接决定减速比和电机选型，也是后面所有 PD 参数的物理上限——增益调再大，电机出不了这个力矩也是白搭。

2.2 把腿型写成一张参数表：连杆长度、质心与惯量

仿真里翻车最多的原因，是设计文档只写了“大腿 400 mm”，没写质心在哪、惯量多少。控制算法对连杆长度不敏感，对质心位置极其敏感。我的习惯是把腿部参数固化成一份配置文件，机械改一版就改一次，控制侧只读这份文件。

# 双足平台单腿参数表，单位：m / kg / kg·m^2，向下为 z 负方向 link_thigh: length: 0.40 # 髋 pitch 轴到膝轴的距离 mass: 2.30 com: [0.0, 0.0, -0.19] # 质心相对髋轴，压得越靠上越省力矩 inertia: [0.021, 0.022, 0.004] link_shank: length: 0.38 mass: 1.60 com: [0.0, 0.0, -0.17] inertia: [0.014, 0.015, 0.003] link_foot: length: 0.22 # 脚长，决定支撑多边形前后边界 width: 0.10 # 脚宽，决定侧向稳定余量 mass: 0.55 ankle_height: 0.07 # 踝轴离地高度 joint_limit: # 单位 rad，写死在配置里防止轨迹越界 hip_pitch: [-1.05, 0.90] knee: [0.00, 2.40] ankle_pitch: [-0.90, 0.60] total_mass: 18.0

这份表里有三条经验规则值得抄：大腿与小腿长度比控制在 1.0～1.1，比例过偏会让摆动相惯量集中在一端，滤波很难做；踝轴离地高度取脚长的 0.3 倍左右，力臂太短则踝关节需要极大力矩才能压住重心；脚宽不要小于 0.09 m，否则侧向稳定余量只剩几毫米，一点侧风或线缆拉扯就出去了。

质心位置要单独强调。同样质量、同样长度的腿，质心从 -0.19 挪到 -0.23，髋关节静态力矩会涨两成，而这一成往往就是电机选型从 30 N·m 档跳到 40 N·m 档的分界线。省钱的办法是把电池和驱动器往髋部以上放，用躯干配重把整机质心往上抬，而不是硬堆关节力矩。

2.3 用正向运动学校验设计参数：一段脚本查三个死角

参数表写完别急着出图，先跑一遍正向运动学，把三个地方查掉：站立高度下膝关节有没有接近奇异位形、踝关节在最大步长时够不够用、脚尖落点是否落在设计的工作空间里。

import math def fk_leg(q, l1, l2, ankle_h): """q = [hip_pitch, knee, ankle_pitch]，返回踝轴与脚尖在髋坐标系下的位置。 约定：x 向前，z 向下为正。""" hip, knee, ank = q # 膝角以伸直为 0，向前弯为正，所以小腿绝对角 = hip + knee x_ankle = l1 * math.sin(hip) + l2 * math.sin(hip + knee) z_ankle = l1 * math.cos(hip) + l2 * math.cos(hip + knee) # 脚尖：踝轴再往前伸 ankle_h，随踝角旋转 x_toe = x_ankle + ankle_h * math.sin(hip + knee + ank) z_toe = z_ankle - ankle_h * math.cos(hip + knee + ank) return (x_ankle, z_ankle), (x_toe, z_toe) if __name__ == "__main__": l1, l2, ah = 0.40, 0.38, 0.07 # 检查 1：站立姿态 hip=0.35, knee=0.60, ankle=-0.25 时的踝高 ankle, toe = fk_leg([0.35, 0.60, -0.25], l1, l2, ah) print("踝轴", ankle, "脚尖", toe) # 检查 2：膝角扫到 0.05 rad 附近时，脚的位置是否已经贴到髋正下方 for k in (0.05, 0.30, 0.60, 1.20): _, t = fk_leg([0.0, k, 0.0], l1, l2, ah) print(f"knee={k:.2f} 脚尖x={t[0]:.3f} 脚尖z={t[1]:.3f}")

这段脚本只做一件事：把关节角映射成落点。使用时把站立姿态那组角填进去，看算出来的 z 是不是等于你预期的质心高度（减去踝高）。如果差 3 cm 以上，说明参数表的长度或踝高写错了，这种错误带进仿真会表现成“机器人总是比模型矮一截”，调半天增益都没用。

第二个用法是扫膝角。当膝角接近 0，腿接近伸直，雅可比矩阵条件数迅速变大，同样的关节角误差会放大成很大的落点误差——这就是典型的奇异位形。所以站立姿态的膝角不要小于 0.4 rad，行走时也把下限卡在 0.25 rad，宁可牺牲一点腿长利用率。参数直接写进配置文件的 joint_limit 里，轨迹生成器越界就报错，比在实机上撞限位强。

3. 步态规划落地：把步长、步频、质心高度算成一组数

控制侧最常见的误会，是以为步态规划是在“画一条好看的曲线”。实际上它是在一组物理约束里挑一个可行点：质心高度决定倒立摆的时间常数，时间常数决定单支撑能撑多久，单支撑时长乘速度就是步长。这几个量互相锁定，改一个必须重算其余。

3.1 线性倒立摆给出步长、步频与质心高度的可行域

把整机当成一个质点挂在可伸缩的腿上，质心高度恒定，横向不管，纵向就有 ω = sqrt(g/h) 和特征时间 T_c = sqrt(h/g)。h = 0.62 m 时 T_c ≈ 0.251 s。经验上单支撑时长取 1.1～1.4 倍 T_c，太短则重心来不及从一只脚换到另一只脚，太长则整机开始往支撑脚外侧倒。

表里“步长 = 速度 × 步周期”这一列是给机构组看的下限检查：如果算出来的步长超过髋关节行程能覆盖的一半，说明要么加速度太大，要么腿太短，回去改参数，别在控制里硬凑。

还有一个常被忽略的量是抬脚高度。0.03～0.05 m 足够跨过地面接缝；设到 0.10 m 看着漂亮，但摆动相时间不变的情况下垂直速度翻倍，落地冲击会直接传到膝关节，几次之后减速器回差就出来了。我的做法是抬脚高度按步频反比给：步频 2 Hz 给 0.04 m，步频 1.6 Hz 给 0.05 m。

3.2 摆动腿轨迹：三次样条加抬脚约束的最小实现

落脚点和抬脚高度定完，中间用三次多项式插值就够，不必上五次。边界条件是起点和终点的位置、速度，速度都给 0，端点加速度不连续带来的冲击在 20 kg 级平台上基本看不出来，反而省了一半计算量。

import math def cubic(p0, v0, p1, v1, T): """返回一个 t -> 位置 的函数，t ∈ [0, T]。 p0/p1 起终点位置，v0/v1 起终点速度，T 段时长。""" a0 = p0 a1 = v0 a2 = (3 * (p1 - p0) - T * (2 * v0 + v1)) / T**2 a3 = (2 * (p0 - p1) + T * (v0 + v1)) / T**3 return lambda t: a0 + a1 * t + a2 * t * t + a3 * t * t * t def swing_foot(x0, x1, lift, T, n=50): """摆动相：x 方向从 x0 到 x1，z 方向抬 lift 再落回。""" fx = cubic(x0, 0.0, x1, 0.0, T) # 水平：单调过渡 # 垂直：用两段拼，前半抬到 lift，后半落回，保证顶点速度为 0 up = cubic(0.0, 0.0, lift, 0.0, T / 2) down = cubic(lift, 0.0, 0.0, 0.0, T / 2) traj = [] for i in range(n + 1): t = T * i / n z = up(t) if t <= T / 2 else down(t - T / 2) traj.append((fx(t), z, t)) return traj if __name__ == "__main__": for x, z, t in swing_foot(-0.11, 0.11, 0.04, 0.30)[::10]: print(f"t={t:.2f}s x={x:+.3f}m z={z:.3f}m")

参数怎么改：x0 / x1取 ±半个步长，符号按左右腿相位取反；lift就是上节说的 0.03～0.05 m；T用单支撑时长，注意摆动相时长必须严格小于单支撑时长，留 20% 余量给双支撑切换，否则两条腿会同时在空中。

这段代码有一个隐藏假设：水平方向匀速过渡。真实机器人上更稳的做法是让水平轨迹在起步阶段慢、中段快，用速度曲线把落脚冲击分散开。想改进的话，把fx换成带峰值速度约束的分段多项式，峰值不超过 2 倍平均速度即可，太大反而会在落地瞬间产生反向冲击。

3.3 ZMP 判据：判断这一步会不会摔的硬指标

轨迹生成完必须过一遍零力矩点检查。质心高度恒定、忽略角动量变化时，简化判据是：

x_zmp = x_com - (z_com / g) * x_com_ddot y_zmp = y_com - (z_com / g) * y_com_ddot

把(x_zmp, y_zmp)逐帧算出来，看它有没有跑出支撑多边形。单支撑时支撑多边形只有一只脚，前后边界是脚尖和脚跟，左右边界是脚宽；双支撑时是两只脚的凸包，中间那块“桥”区域也算安全。实操里我会留 15 mm 的安全边距，算出来的 ZMP 贴边就直接报警，不要等到实机上才发现。

判据本身不难，难在加速度怎么来。用轨迹二次差分算出来的加速度噪声极大，必须做低通滤波，截止频率取步频的 3～5 倍（步频 2 Hz 就取 8 Hz 左右）。滤太低会把真实的冲击抹掉，滤太高则 ZMP 曲线全是毛刺，看不出趋势。这一步做完，你就有了一个不需要上实机就能判断“这组步态参数靠不靠谱”的工具。

4. 关节闭环调参：PD 增益从仿真搬到实机的三步折减

关节调参是整份设计文档里最容易写成玄学的一章。有人给一串数说“这样能走”，换台机器就废。根本原因是增益不是独立参数，它跟等效惯量、目标带宽、采样周期绑在一起。把这层关系写清楚，参数才有可移植性。

4.1 三个环的带宽分配：电流、速度、位置谁先调

驱动器内部通常有电流环，外面套速度环，再外面是位置环或力矩环，最外层才是步态。带宽必须逐级递减，一般差 5～10 倍，否则外环会激励内环的谐振。

新手最容易犯的错是先调位置环。位置环输出的是力矩指令，如果电流环没整定好，位置环给多少力矩都会被电流环的滞后吃掉，表现为“加增益没反应，再加就振”。正确顺序是从内到外，每一级单独做阶跃响应，看上升时间和超调，达标了再往外走。

还有一个必须提的：通信周期。步态外环跑 500 Hz，但关节驱动器如果只支持 200 Hz 收指令，中间就必须做指令插值，否则关节拿到的是阶梯状目标位置，微分项直接炸。这个坑我在两台上都踩过，配置里写的是 1 kHz，实际驱动器分频到 200 Hz，查了半天才发现。

4.2 单关节 PD 加重力项：一段能直接跑的离线仿真

调参前先在离线环境里把数量级摸出来，比在实机上瞎试快十倍。下面这段只仿真一个关节，负载是重力，用来验证 Kp、Kd 的量级和力矩上限是否合理。

import math J = 0.045 # 关节等效惯量 kg·m^2（含减速器折算） M = 2.30 # 连杆质量 kg LC = 0.19 # 质心到关节轴距离 m G = 9.81 TAU_MAX = 35.0 # 关节峰值力矩 N·m，来自 2.1 节的估算 def gravity(q): return M * G * LC * math.sin(q) def sim(kp, kd, q_des=0.40, dt=1e-4, T=1.5): q, qd, t, tau_peak = 0.0, 0.0, 0.0, 0.0 while t < T: # PD + 重力补偿，输出力矩先限幅再进动力学 tau = kp * (q_des - q) - kd * qd + gravity(q) tau = max(-TAU_MAX, min(TAU_MAX, tau)) qdd = (tau - gravity(q)) / J qd += qdd * dt q += qd * dt t += dt tau_peak = max(tau_peak, abs(tau)) return q, tau_peak if __name__ == "__main__": # 目标带宽 35 rad/s、阻尼比 0.9：kp = w^2*J，kd = 2*zeta*w*J w, zeta = 35.0, 0.9 kp = w * w * J # ≈ 55.1 kd = 2 * zeta * w * J # ≈ 2.84 q, peak = sim(kp, kd) print(f"kp={kp:.1f} kd={kd:.2f} 终值={q:.3f} 峰值力矩={peak:.1f}")

逻辑很简单：力矩指令就是 PD 加前馈重力项，限幅后进单惯量动力学。跑出来终值应该接近 0.40，峰值力矩在 20 N·m 左右，说明参数没超物理边界。如果峰值贴着 35 上限，说明 Kp 太大或目标角阶跃太猛，实机上会听到“咚”的一声然后触发过流。

参数含义要拆开看：J不是电机转子惯量，是折算到关节侧的等效惯量，减速比平方会把它放大很多倍，估错一个量级增益就全废；kp = w²·J决定了闭环刚度，kd = 2ζwJ决定阻尼，ζ 取 0.8～1.0，低于 0.7 会明显超调，高于 1.2 则响应发钝。

4.3 从仿真到实机的三步折减

仿真得到的增益直接搬上实机，十有八九会振。原因不在模型错，而在实机多了三样东西：减速器回差、编码器噪声、结构柔性。我的折减流程固定三步。

第一步，Kp 先打 0.6～0.7 折。仿真的 J 通常偏小（没算上谐波减速器的柔性和线缆），刚度一上来就把结构谐振激励起来了。打完折先做小角度阶跃（5°），看有没有 200 Hz 以上的高频分量，有就继续降。

第二步，Kd 按噪声反推。Kd 直接放大编码器噪声，实机上把 Kd 从仿真值往下调，直到关节静止时听不到“沙沙”声。经验值大概在仿真值的 0.5～0.8 之间，编码器分辨率越低折得越狠。

第三步，加三重限幅。力矩限幅取峰值的 80%，速度限幅取设计最大转速的 90%，位置指令变化率限幅（每周期不超过 0.5°）。这三道闸门看起来保守，实际调试期能挡住绝大多数“一个尖峰就把腿甩出去”的事故。

折减完再做一次带宽验证：给关节一个 1° 的小方波，看响应的上升时间是不是和设计带宽对得上。如果实测比设计慢一倍以上，多半是通信周期或滤波拖的，回去查链路，别再加 Kp。

5. 排查：双足机器人设计里最容易翻车的五个现场问题

下面五条都是实机上真出过的，每条按现象、原因、解决写。

5.1 站立时整机高频嗡鸣，脚底发麻

现象：静止站立时能听到持续的高频声，手摸脚底有明显麻感，示波器上关节电流是等幅振荡。

原因：位置环 Kp 过高，或者 Kd 把编码器噪声放大后形成自激。还有一种可能是电流环带宽不够，位置环的指令被滞后，形成相位裕度不足的正反馈。

解决：先把 Kd 降到 0 看振荡是否消失，消失就是噪声放大，加一阶低通（截止 50 Hz）再逐步回升；不消失就把 Kp 砍半，重新做小角度阶跃。如果两者都无效，检查驱动器电流环参数是不是被改过，恢复默认再试。

5.2 抬腿瞬间整机往支撑腿外侧倒

现象：摆动腿刚离地，整机在 0.2 s 内向外侧倾斜，靠踝 roll 勉强拉回来，动作很别扭。

原因：质心横向偏移量给早了或给多了。很多实现里在抬腿前就把质心往外推，但支撑腿的踝 roll 行程只有 ±20°，推过量后踝先到限位，剩下靠髋 roll 硬撑，力矩方向就错了。

解决：把横向偏移改成随摆动相进度线性增加，峰值不超过脚宽的一半；同时把踝 roll 的限位报警阈值设到行程的 80%，先报警再限幅。真要提速，就加宽脚掌，别指望控制补。

5.3 落脚瞬间膝盖“打软”，支撑腿迅速下沉

现象：脚一触地，膝盖角度在 30 ms 内掉了 5°～8°，整机高度明显下沉，然后靠 PD 拉回来，形成一次弹跳。

原因：触地检测延迟，重力前馈在触地瞬间还按摆动相算，力矩给少了；或者膝 Kp 在支撑相不够，压不住冲击载荷。

解决：加一个基于关节力矩突变或足底开关的触地检测，检测到就把重力前馈切成双支撑模型；同时把支撑相的膝 Kp 提高 20%～30%，并对落地前后 50 ms 的力矩指令做限速。别用位置环硬顶，越顶越弹。

5.4 仿真里走得稳，实机第一步就跪

现象：同一组参数，仿真连续走十步不掉，实机迈出第一步膝盖就弯下去，整机前倾。

原因：仿真里的地面是刚性、无滑动的，实机脚底橡胶有压缩，等效腿长比模型长 5～10 mm；再加上 IMU 安装角有偏差，姿态估计把前倾当成了竖直。

解决：先量一遍实机站立高度，把模型里的踝高和脚底压缩量补进去；再静态标定 IMU 零偏，静止放 30 s 取均值写进配置。这两步做完，多数“仿真能走实机不能”的问题能消掉一半。

5.5 电量下降后步态逐渐变形

现象：满电时走得挺稳，电压掉到 80% 后开始出现落脚不准、抬脚变矮。

原因：电压下降导致同样占空比下输出力矩变小，而位置环的增益是按满电整定的，等效刚度下降后跟踪误差变大；如果摆动相轨迹是纯位置控制的，抬脚高度自然就矮了。

解决：把母线电压做成前馈项补偿到力矩指令里，或者直接改用转矩控制模式让力矩指令与电压解耦；同时在摆动相轨迹里加末端位置闭环修正，用实测关节角反算实际落点。这条最容易被忽略，因为它是渐变的，不炸不响，只是越走越差。

6. 验证节奏：用四级台阶把“能站”推到“连走三步”

双足项目最容易出的事故是在整机刚装好、参数还没调完时就急着让它走，结果一次摔倒把踝关节减速器打坏，返修两周。我后来的习惯是把验证拆成四级台阶，每级都有明确的通过标准，不达标绝不进下一级。

一个提速技巧：在三级之前就把 ZMP 在线计算接进上位机，用实时曲线而不是日志做事后分析。曲线贴边时你能立刻看到是哪个方向的加速度超了，改轨迹还是改落脚点一目了然。事后翻日志经常看不出因果，因为落脚点和质心是错相的。

我自己的教训是：每次想“就试一步应该没事”，那一把一定出事。后来定了条死规矩，任何参数改动后先过一级台阶，再往上升。慢是慢了点，但省下来的返修时间远超调试时间。希望帮到你。

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