简介：这份文档面向双足机器人控制方向的开发者与嵌入式学习者，围绕北京汉库科技HGR-3HV-C型号机器人，系统讲解其步伐源代码，帮助读者理解并优化机器人的运动控制逻辑。内容从初始站立子程序切入，逐层拆解双腿下蹲、起立、侧身及抬腿等单一动作的变速运动子程序，按第1族、第2族分阶段说明电机速度与关节角度的协调方式，并涉及姿态检测、传感器融合、动力学模型与控制算法等配套模块，便于开发者调整参数、定制步态并提升动态性能与能源效率。资源包共1个doc文件，约9.07MB，为完整手册文档，目录结构清晰，适合按章节检索学习。目前已有474人学习下载，可作为研究双足机器人步态控制与嵌入式程序实现的参考材料。

1. 双足机器人设计：从一条腿的平衡开始

双足机器人设计最反直觉的地方在于：它根本不是先画两条腿，而是先让一条腿站稳。很多刚接触这个方向的人会直接打开三维软件，画髋关节、膝关节、踝关节，拼出一个像人的结构，然后发现它连站都站不住。问题不在结构，在于控制周期和质心投影的关系没理清。双足机器人设计的核心矛盾是：机械结构决定了你能走多快，控制频率决定了你能站多久。一个典型的入门级双足平台，整机重量在 1.5 到 3 公斤之间，腿长 200 到 350 毫米，关节数 10 到 12 个，控制板跑 500Hz 到 1kHz 的闭环。这个量级下，你不需要昂贵的力矩电机，但你需要把零力矩点（ZMP）和倒立摆模型吃透。适合谁看？做过轮式或四足、想切到双足方向的嵌入式工程师，以及手里有 3D 打印机和舵机、想认真做一个能走路的平台的开发者。接下来按“先建模、再选型、后调参”的顺序，把这条腿站稳。

2. 双足机器人设计的运动学建模与关节选型

2.1 用倒立摆模型把双足问题降维

双足机器人的完整动力学模型有几十个状态变量，直接上 MPC 在入门级 MCU 上跑不动。常见做法是先用线性倒立摆（LIPM）把问题简化：把整机质量集中到质心，假设腿是无质量的伸缩杆，只考虑质心在水平面的运动。这个模型下，质心高度 z 固定，水平加速度与质心到 ZMP 的偏差成正比。公式不复杂，关键是它给了你一个可解析的 ZMP 轨迹生成方法。

我一般会先在 Python 里把 LIPM 的步态轨迹跑一遍，确认质心轨迹和 ZMP 轨迹的相位关系，再决定控制周期。比如步频 1.5Hz、步长 80mm 的情况下，质心在支撑腿上方时 ZMP 要落在脚掌中心，摆动腿落地前 ZMP 要提前移到新支撑脚。这个提前量就是控制周期的下限来源：如果你的控制周期是 2ms，ZMP 调整的相位滞后大约 3 到 5 度，还能接受；如果周期到 10ms，滞后会超过 15 度，机器人就会在落地瞬间前后晃。

import numpy as np # LIPM 参数 z = 0.25 # 质心高度，单位米 g = 9.81 # 重力加速度 T = 1.0 / 1.5 # 步态周期，步频 1.5Hz dt = 0.002 # 控制周期 2ms # 离散化 LIPM 状态转移 omega = np.sqrt(g / z) A = np.array([[np.cosh(omega*dt), np.sinh(omega*dt)/omega], [omega*np.sinh(omega*dt), np.cosh(omega*dt)]]) B = np.array([[1 - np.cosh(omega*dt)], [-omega*np.sinh(omega*dt)]]) # 给定 ZMP 参考，递推质心轨迹 def generate_com(zmp_ref, x0, v0, steps): x, v = x0, v0 traj = [] for i in range(steps): u = zmp_ref[i] x, v = A @ np.array([x, v]) + B.flatten() * u traj.append((x, v)) return np.array(traj) # 示例：ZMP 在脚掌中心附近小幅摆动 zmp = 0.02 * np.sin(2 * np.pi * np.arange(500) * dt / T) com = generate_com(zmp, 0.0, 0.0, 500) print(com[:5])

这段代码的作用是验证你的控制周期是否足够小。参数z是质心高度，直接决定倒立摆的自然频率；dt是控制周期，越小相位滞后越小，但对 MCU 算力要求越高。zmp_ref是期望的 ZMP 轨迹，实际调试时这个轨迹来自步态规划器。跑完这段代码你会看到，质心轨迹的幅值大约是 ZMP 幅值的 1.5 到 2 倍，这个比例关系在调参时很有用：如果你希望质心摆动不超过 30mm，ZMP 摆动就要控制在 15 到 20mm。

2.2 关节选型：舵机、无刷电机还是准直驱

双足机器人设计的关节选型直接决定你能不能做力控。常见方案有三类：舵机、无刷电机加谐波减速器、准直驱电机。舵机便宜、集成度高，但只能做位置控制，背隙大，落地冲击全靠结构硬扛。无刷加谐波减速器力矩密度高、背隙小，但价格是舵机的五到十倍，而且需要额外的力矩传感器才能做柔顺控制。准直驱电机扭矩密度低，但反驱透明度好，适合做基于电流环的力控。

我一般会按这个顺序选：如果预算在两千元以内，用数字舵机，但要把控制频率提到 500Hz 以上，用位置环加前馈补偿来弥补背隙。如果预算到一万元，选准直驱电机，配合关节力矩估算做阻抗控制，落地冲击会小很多。谐波减速器方案适合预算充足且需要高精度位置控制的场景，但入门阶段不建议，因为调试复杂度高，一旦减速比选错，整个步态频率都要重调。

选型时还要看一个参数：关节最大角速度。双足行走时摆动腿的髋关节角速度峰值大约在 300 到 500 度每秒，如果电机额定转速不够，步频就上不去。舵机标称的 0.1 秒每 60 度换算过来是 600 度每秒，但那是空载，带载后会掉到一半以下。所以选舵机时要把扭矩余量留到 2 倍以上。

2.3 用 URDF 把连杆参数固化下来

运动学建模的最后一步是把连杆长度、质心位置、关节轴方向写成 URDF 文件。这个文件不只是给仿真用的，它也是你后续做正逆运动学解算的输入。常见做法是先在三维软件里量出实际尺寸，再手写 URDF，然后用 Pinocchio 或 KDL 做验证。

<!-- 简化版单腿 URDF，只保留髋、膝、踝三个关节 --> <robot name="leg"> <link name="hip"> <inertial> <mass value="0.3"/> <origin xyz="0 0 0"/> </inertial> </link> <joint name="hip_joint" type="revolute"> <parent link="hip"/> <child link="thigh"/> <axis xyz="0 1 0"/> <limit lower="-1.2" upper="1.2" effort="10" velocity="8"/> </joint> <link name="thigh"> <inertial> <mass value="0.4"/> <origin xyz="0 0 -0.12"/> </inertial> </link> <joint name="knee_joint" type="revolute"> <parent link="thigh"/> <child link="shank"/> <axis xyz="0 1 0"/> <limit lower="-2.0" upper="0.1" effort="10" velocity="8"/> </joint> <link name="shank"> <inertial> <mass value="0.3"/> <origin xyz="0 0 -0.12"/> </inertial> </link> </robot>

这段 URDF 的关键参数是origin xyz和axis xyz。origin决定质心在连杆坐标系里的位置，写错了会导致逆动力学算出来的力矩偏差很大。axis决定关节旋转轴，双足机器人的髋关节通常有横滚、俯仰、偏航三个轴，入门阶段先做俯仰和横滚，偏航用被动关节或者小舵机补偿。limit里的effort和velocity要和实际电机匹配，写大了仿真会失真，写小了仿真里机器人走不动。

3. 双足机器人设计的步态规划与 ZMP 轨迹生成

3.1 用三次样条生成摆动腿轨迹

步态规划分两部分：支撑腿的 ZMP 轨迹和摆动腿的关节轨迹。摆动腿从离地到落地，要满足三个约束：起落点位置、离地高度、落地速度接近零。常见做法是用三次样条插值，在笛卡尔空间规划足端轨迹，再用逆运动学解算关节角。

import numpy as np from scipy.interpolate import CubicSpline def swing_foot_traj(p_start, p_end, height, T_swing, dt): """生成摆动腿足端轨迹，p_start/p_end 为三维坐标""" t = np.arange(0, T_swing, dt) # 水平方向线性插值 x = np.linspace(p_start[0], p_end[0], len(t)) y = np.linspace(p_start[1], p_end[1], len(t)) # 垂直方向用抛物线保证离地高度 z = p_start[2] + (p_end[2] - p_start[2]) * t / T_swing \ + height * 4 * t * (T_swing - t) / (T_swing ** 2) return np.column_stack([x, y, z]) # 示例：步长 80mm，离地 40mm，摆动时间 0.3s traj = swing_foot_traj( np.array([0.0, 0.0, 0.0]), np.array([0.08, 0.0, 0.0]), height=0.04, T_swing=0.3, dt=0.002 ) print(traj.shape) # (150, 3)

这段代码里height是摆动腿离地最大高度，一般取步长的 0.4 到 0.6 倍。太小会蹭地，太大会增加落地冲击。T_swing是摆动时间，通常占整个步态周期的 40% 到 50%。dt和控制周期一致，保证轨迹点能直接送给逆运动学。跑完这段你会得到 150 个足端位置点，下一步就是逐点解逆运动学。

3.2 逆运动学解算与关节角限幅

单腿的逆运动学在矢状面内可以解析求解。给定髋关节到足端的距离和方向，用余弦定理算膝关节角，再算髋关节角。注意膝关节通常只有一个自由度，且只能向后弯，所以解算时要判断符号。

def leg_ik(hip_pos, foot_pos, l1, l2): """矢状面内单腿逆运动学，返回髋、膝角度""" dx = foot_pos[0] - hip_pos[0] dz = foot_pos[2] - hip_pos[2] dist = np.sqrt(dx**2 + dz**2) # 可达性检查 if dist > l1 + l2 or dist < abs(l1 - l2): return None # 余弦定理求膝关节角 cos_knee = (l1**2 + l2**2 - dist**2) / (2 * l1 * l2) knee = np.pi - np.arccos(np.clip(cos_knee, -1, 1)) # 求髋关节角 alpha = np.arctan2(dx, -dz) beta = np.arccos(np.clip((l1**2 + dist**2 - l2**2) / (2 * l1 * dist), -1, 1)) hip = alpha + beta return hip, knee # 示例：大腿 120mm，小腿 120mm hip, knee = leg_ik( np.array([0.0, 0.0, 0.25]), np.array([0.04, 0.0, 0.05]), l1=0.12, l2=0.12 ) print(f"hip: {np.degrees(hip):.1f} deg, knee: {np.degrees(knee):.1f} deg")

l1和l2是大腿和小腿长度，必须和 URDF 里一致。dist是髋到足端的距离，超出l1+l2说明目标点不可达，这时要截断或者调整步长。knee算出来是正数，表示膝关节向后弯，符合人体结构。实际下发前还要做关节限幅，把角度限制在 URDF 的limit范围内，否则舵机会堵转。

3.3 ZMP 轨迹与步态相位对齐

ZMP 轨迹要和支撑脚切换时刻对齐。常见做法是把一个步态周期分成四个相位：双支撑、单支撑左、双支撑、单支撑右。ZMP 在单支撑期从脚掌后跟移到前掌，双支撑期从一只脚移到另一只脚。这个轨迹可以用分段线性或者五次多项式生成。

我一般会在代码里维护一个相位计数器，每个控制周期根据当前相位查表得到 ZMP 参考值。表里的数据来自离线规划，在线只做插值。这样 MCU 的算力消耗很小，1kHz 下也就几十个时钟周期。关键是相位切换的时刻要和足端落地检测对齐，否则 ZMP 提前或滞后都会导致机器人前后晃。落地检测可以用足底压力传感器，也可以用关节电流突变来判断，后者不需要额外传感器，但阈值要调。

4. 双足机器人设计的控制框架与实时调参

4.1 用 PD 加前馈做关节位置控制

入门阶段最稳的控制框架是关节空间 PD 加前馈。位置环用 PD，速度前馈来自步态规划的关节角速度，力矩前馈来自逆动力学。这样即使 PD 增益不高，跟踪误差也能压到 2 度以内。

// 单关节 PD + 前馈，运行在 1kHz 中断里 typedef struct { float kp; float kd; float target_pos; float target_vel; float target_torque; } JointCtrl; float joint_update(JointCtrl *c, float pos, float vel, float dt) { float err_pos = c->target_pos - pos; float err_vel = c->target_vel - vel; float torque = c->kp * err_pos + c->kd * err_vel + c->target_torque; // 力矩限幅，保护电机 if (torque > 10.0f) torque = 10.0f; if (torque < -10.0f) torque = -10.0f; return torque; }

kp和kd的整定顺序是：先把kd设为零，慢慢加kp直到关节开始等幅振荡，然后取振荡临界值的 0.5 倍作为kp。再加kd，从kp的 0.1 倍开始加，直到振荡消失。target_torque前馈项在摆动腿加速阶段很重要，没有它跟踪误差会到 5 度以上。dt必须和实际控制周期一致，写错了积分项会漂。

4.2 用 IMU 做姿态反馈与落地检测

IMU 提供躯干俯仰和横滚角，用来修正 ZMP 参考。常见做法是串级控制：外环用姿态误差修正质心位置参考，内环还是关节 PD。IMU 的安装位置要尽量靠近质心，减少振动耦合。数据融合用互补滤波就够了，双足机器人的姿态变化频率不高，不需要卡尔曼。

落地检测用 IMU 的垂直加速度突变加上关节电流突变，两个条件同时满足才认为落地。这样可以避免摆动腿加速时的误触发。检测到落地后，相位计数器立即切换到双支撑，ZMP 参考切到新支撑脚。这个切换时刻的精度直接影响步态稳定性，我一般会把 IMU 采样率提到 1kHz，和关节控制同频。

4.3 实时调参：先调站，再调走

调参顺序不能乱。第一步让机器人双脚站立，调 PD 增益到躯干不晃。第二步单腿站立，调 ZMP 参考的偏移量，让质心投影落在支撑脚中心。第三步原地踏步，调摆动腿轨迹的高度和落地检测阈值。第四步才往前走，调步长和步频。

每一步都要记录数据：关节角跟踪误差、IMU 姿态角、ZMP 偏差。我习惯用串口把数据打到上位机，用 Python 画实时曲线。调参时一次只改一个参数，改完走十步看是否发散。常见翻车点是步长加得太快，ZMP 轨迹没跟上，机器人直接前扑。这时候不要急着加 PD，先检查 ZMP 相位对不对。

5. 双足机器人设计的避坑与常见问题排查

5.1 机器人站立时高频抖动

现象：双脚站立时躯干和腿有 20Hz 以上的高频抖动，手摸电机能感觉到。原因通常是 PD 的kd太小或者控制周期抖动。解决：先把kd加到kp的 0.2 倍，如果还抖，检查控制中断是否被其他任务打断。用示波器测中断周期，抖动超过 10% 就要把无关任务挪到低优先级。

5.2 行走时前后晃，越走越不稳

现象：机器人能迈步，但躯干前后摆幅逐渐增大，几步后失稳。原因多半是 ZMP 轨迹和实际支撑脚切换时刻没对齐。解决：打印 ZMP 参考和实际足底压力中心的曲线，看相位差。如果 ZMP 提前，把相位计数器切换阈值调后；如果滞后，调前。另一个可能是质心高度z设得比实际高，LIPM 频率算错，重新量质心位置。

5.3 落地时膝关节过冲，舵机发出异响

现象：摆动腿落地瞬间膝关节角度超过目标值，舵机堵转异响。原因是落地冲击力矩超过了 PD 输出限幅，关节被外力带着走。解决：在落地前 50ms 把膝关节的kd临时提高 2 倍，增加阻尼。或者加落地缓冲轨迹，让足端在触地前速度降到接近零。如果还不行，检查target_torque前馈是否在落地时刻有突变，有的话做低通滤波。

5.4 逆运动学解算失败导致关节角跳变

现象：行走中某个关节角突然跳到限幅值，机器人踉跄。原因是足端轨迹点超出了腿的可达范围，leg_ik返回None后没有处理。解决：在逆运动学里加可达性检查，不可达时把足端位置往髋关节方向拉回，直到可达。同时限制步长和抬腿高度，留 10% 的余量。这个坑很隐蔽，因为离线规划时轨迹是可达的，但在线时 IMU 修正量叠加后可能超限。

5.5 电池电压下降后步态变差

现象：满电时走得好，电压降到 7V 以下后步频变慢、步幅变小。原因是舵机扭矩随电压下降，PD 输出同样的力矩指令，实际关节角跟踪不上。解决：在控制里加电压补偿，把 PD 输出乘以额定电压和实际电压的比值。或者用电流环做力矩控制，但舵机不支持。简单做法是电压低于阈值时自动降低步频和步长，牺牲速度保稳定。

6. 双足机器人设计的进阶：从走到抗扰

走到这一步，机器人已经能直线行走，但离“有用”还差一个抗扰能力。进阶方向有三个：推力恢复、斜坡适应、外力柔顺。推力恢复最简单的是用捕获点（Capture Point）做紧急步，检测到躯干速度超过阈值时，摆动腿提前落地，落点选在捕获点位置。捕获点公式是质心速度乘以根号下质心高度除以重力加速度，加上质心当前位置。这个计算量很小，1kHz 下完全跑得动。

斜坡适应需要估计地面倾角。常见做法是用 IMU 的俯仰角加上足底压力中心偏移来估算，然后修正 ZMP 参考的垂直分量。我一般会在站立时先做一次地面标定，把 IMU 零偏和地面倾角一起估出来，行走时只做小范围修正。外力柔顺则是把关节 PD 改成阻抗控制，给定一个虚拟弹簧刚度，外力推的时候关节顺着让，外力撤了弹回来。刚度参数从 20Nm/rad 开始试，太大没效果，太小站不住。

验证抗扰能力的方法很直接：站在机器人侧面，用手掌推躯干，力度从 10N 加到 50N，看它能不能在两步内恢复。记录恢复步数和最大躯干倾角，这两个指标比看它走得多快更有意义。我自己的习惯是每次改完控制参数，先推十次，有一次没恢复就回去查数据。双足机器人设计这件事，走起来只是及格，推不倒才算入门。希望帮到你。

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