四足机器人步态控制与PyBullet仿真实战：从单腿摆动到Trot步态

最近机器人圈里讨论度很高的话题，莫过于“机器人跑步速度突破”这类新闻。尤其当国内机器人被拿来和博尔特的百米纪录对比时，很多人都会好奇：机器人到底是怎么跑起来的？这背后其实是一套非常典型的运动控制技术栈，包括四足/双足运动控制、步态规划、关节驱动和仿真验证。

这篇文章会从“机器人跑得快”这个现象切入，把运动控制中最核心的步态控制与仿真方法拆开讲一遍。我会使用 PyBullet 作为仿真环境，从一个可以运行的“单腿摆动”示例讲起，再扩展到四足机器人的 Trot 步态设计思路。无论你是机器人方向的在校学生，还是刚接触运动控制的开发工程师，这篇文章都会给出可以直接照做的代码和避坑建议。

1. 背景与核心概念

1.1 机器人跑得快，到底难在哪里

很多人看到机器人跑步视频，第一反应是“机械结构真牛”。但真正让机器人跑起来的，不只是硬件，还有控制算法。

跑步动作的本质是“在高速运动中不断调整身体姿态，同时完成腿部的周期性摆动与触地支撑”。这比走路难得多，因为：

• 速度快意味着单脚支撑时间更短，控制器必须在几十毫秒内完成一次决策。
• 高速奔跑时惯性力很大，单纯依赖预设轨迹不够，必须引入动力学反馈。
• 机器人需要在“稳定支撑”和“快速摆动”之间频繁切换，步态相位一旦乱掉就会摔倒。

所以，机器人跑步更像是一个“运动控制 + 实时计算 + 机械执行”的综合问题。这也是为什么近几年的机器人团队往往同时具备控制、算法和机械设计能力。

1.2 四足机器人是什么

四足机器人就是有四条腿的机器人，常见的有四足仿生机器人、工业巡检机器人等。相较双足机器人，四足机器人在静态稳定性上更有优势，因为它更容易让重心落在支撑多边形内；但在高速运动时，它同样会进入“动态不稳定”状态，需要靠控制算法维持平衡。

四足机器人的一条典型腿结构包含髋关节和膝关节，高端机型还会有踝关节。这类关节通常由电机加减速器驱动，控制周期一般在 500 Hz 到 1000 Hz。真机上每个关节都要实时采集角度、角速度、力矩信号，同时下发控制指令。

1.3 步态控制是什么

步态控制是四足机器人运动控制的核心，它决定了四条腿什么时候抬起、什么时候踩地。

常见步态包括：

以 Trot 步态为例，它的典型相位是“左前腿 + 右后腿”同时摆动，然后“右前腿 + 左后腿”同时摆动。这种步态兼顾速度和稳定性，是四足机器人最常用的步态之一。

1.4 为什么需要仿真平台

在做真机调试之前，先用仿真平台验证算法，成本更低、风险更小。常见仿真平台有 Gazebo、PyBullet、MuJoCo、Webots 等。

PyBullet 的优势在于：

• 安装简单，纯 Python 调用。
• 支持 URDF 模型导入。
• 自带物理引擎，可以模拟重力、碰撞、摩擦。
• 适合教学和快速原型验证。

这篇文章的实战部分使用 PyBullet 演示；如果你使用 ROS 2，也可以把同样的控制逻辑迁移到 Gazebo 中，核心控制算法并不冲突。

2. 环境准备与版本说明

2.1 推荐环境

本文示例在以下环境中验证过，建议尽量保持一致：

• 操作系统：Ubuntu 22.04 LTS
• Python 版本：3.10
• PyBullet 版本：3.x
• ROS 2 版本：Humble（可选，不要求本节启动）

如果你用的是 Windows 或 macOS，PyBullet 也能运行，但部分图形显示和 URDF 路径处理略有差异，建议优先使用 Ubuntu 虚拟机或 WSL2。

2.2 安装 PyBullet

创建虚拟环境，然后安装依赖：

python3 -m venv robot_env source robot_env/bin/activate pip install pybullet numpy

等待安装完成后，可以用一个简单命令验证：

python -c "import pybullet; print(pybullet.getAPIVersion())"

如果能输出版本号，说明安装成功。

2.3 可选：安装 ROS 2

如果后续需要接入 SLAM、导航或视觉识别，可以安装 ROS 2 Humble。安装命令如下：

sudo apt update sudo apt install ros-humble-desktop

安装完成后，建议再安装机器人常用工具包：

sudo apt install ros-humble-urdf ros-humble-xacro ros-humble-rviz2

需要注意的是，ROS 2 版本与 Ubuntu 版本强绑定，安装前一定要确认对应关系。

2.4 示例项目结构

建议先规划好项目目录，方便后续扩展：

quadruped_robot/ ├── robot_model/ │ └── single_leg.urdf ├── scripts/ │ ├── single_leg_demo.py │ ├── gait_planner.py │ └── ik_solver.py └── README.md

这样一个结构既能容纳单腿测试，也能扩展为完整的四足机器人工程。

3. 核心语法、配置或原理拆解

3.1 运动学基础：从关节角到足端位置

正运动学解决的是“已知关节角，求足端位置”的问题。逆运动学则相反，解决“已知期望足端位置，求关节角”的问题。

以一个二维两连杆腿为例：

• L1 是大腿长度。
• L2 是小腿长度。
• θ1 是髋关节角度。
• θ2 是膝关节角度，这里约定为小腿相对大腿的转角。

正运动学公式为：

x = L1 * sin(θ1) + L2 * sin(θ1 + θ2) z = -L1 * cos(θ1) - L2 * cos(θ1 + θ2)

这里 z 轴向下为负，表示足端相对于髋关节的垂直高度。仿真和真机中，一旦确定了几何参数，正运动学可以直接建模；逆运动学则通常用几何法或数值法求解。

3.2 关节控制模式

在 PyBullet 中，关节控制常见有两种模式：

位置控制

p.setJointMotorControl2( bodyUniqueId=robot, jointIndex=joint_id, controlMode=p.POSITION_CONTROL, targetPosition=target_angle, force=50.0 )

位置控制适合轨迹跟踪，但它的抗干扰能力有限。当机器人足端碰到障碍物时，位置控制可能会产生很大冲击力。

力/力矩控制

p.setJointMotorControl2( bodyUniqueId=robot, jointIndex=joint_id, controlMode=p.TORQUE_CONTROL, force=torque )

力矩控制更接近真实机器人关节的工作方式，适合做柔顺控制和动力学控制，但实现难度更高。初学阶段可以先从位置控制开始。

3.3 步态相位与时间分配

步态规划的第一步，是把每条腿的运动状态划分为不同相位：

• 摆动相：腿抬起，离开地面，向前迈步。
• 支撑相：腿落地，支撑身体，相对地面向后移动，推动身体前进。

以 Trot 步态为例，一个完整步态周期的相位分配如下：

这种对角步态的好处是，地面始终有两条对角腿支撑，机器人在前进过程中不会彻底失去支撑。

3.4 PD 控制器的作用

步态规划给出的是“期望轨迹”，但要让关节真正跟随轨迹，还需要控制器。最常用的是 PD 控制器：

tau = Kp * (theta_des - theta) - Kd * dtheta

其中：

• theta_des 是期望关节角。
• theta 是当前关节角。
• dtheta 是当前关节角速度。
• Kp 是比例增益，决定“纠正位置偏差”的力度。
• Kd 是微分增益，决定“阻尼”力度，能抑制抖动。

PD 控制器虽然简单，却是四足机器人运动控制的基础。实际工程中，还会在 PD 控制器外面叠加力控、重力补偿等策略。

4. 完整实战案例：PyBullet 单腿摆动与 Trot 步态设计

4.1 创建项目结构

先创建项目目录：

mkdir -p quadruped_robot/robot_model mkdir -p quadruped_robot/scripts cd quadruped_robot

4.2 编写单腿 URDF 模型

在robot_model/single_leg.urdf中写入以下内容：

<?xml version="1.0"?> <robot name="single_leg"> <link name="base"> <visual> <geometry><box size="0.4 0.2 0.1"/></geometry> <origin rpy="0 0 0" xyz="0 0 0"/> </visual> <collision> <geometry><box size="0.4 0.2 0.1"/></geometry> <origin rpy="0 0 0" xyz="0 0 0"/> </collision> <inertial> <mass value="5"/> <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.05" iyz="0" izz="0.05"/> </inertial> </link> <joint name="hip_joint" type="revolute"> <parent link="base"/> <child link="upper_leg"/> <origin xyz="0.2 0 -0.05" rpy="0 0 0"/> <axis xyz="0 1 0"/> <limit lower="-1.5" upper="1.5" effort="100" velocity="10"/> </joint> <link name="upper_leg"> <visual> <geometry><box size="0.06 0.1 0.4"/></geometry> <origin rpy="0 0 0" xyz="0 0 -0.2"/> </visual> <collision> <geometry><box size="0.06 0.1 0.4"/></geometry> <origin rpy="0 0 0" xyz="0 0 -0.2"/> </collision> <inertial> <mass value="1.0"/> <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/> </inertial> </link> <joint name="knee_joint" type="revolute"> <parent link="upper_leg"/> <child link="lower_leg"/> <origin xyz="0 0 -0.4" rpy="0 0 0"/> <axis xyz="0 1 0"/> <limit lower="-2.0" upper="0" effort="100" velocity="10"/> </joint> <link name="lower_leg"> <visual> <geometry><box size="0.05 0.08 0.35"/></geometry> <origin rpy="0 0 0" xyz="0 0 -0.175"/> </visual> <collision> <geometry><box size="0.05 0.08 0.35"/></geometry> <origin rpy="0 0 0" xyz="0 0 -0.175"/> </collision> <inertial> <mass value="0.8"/> <inertia ixx="0.008" ixy="0" ixz="0" iyy="0.008" iyz="0" izz="0.008"/> </inertial> </link> </robot>

这个模型包含一个基座、一个髋关节、一个膝关节以及大腿和小腿。为了演示方便，我先把基座固定在地上，只让腿部摆动。这样能更清楚地观察关节运动。

4.3 编写单腿控制脚本

在scripts/single_leg_demo.py中写入：

import math import time import pybullet as p import pybullet_data def main(): p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setGravity(0, 0, -9.8) # 加载地面 p.loadURDF("plane.urdf") # 加载单腿模型 robot = p.loadURDF( "single_leg.urdf", basePosition=[0, 0, 0.8], useFixedBase=True ) hip_idx = 0 knee_idx = 1 # 设置关节位置控制参数 p.setJointMotorControl2(robot, hip_idx, p.POSITION_CONTROL, force=100) p.setJointMotorControl2(robot, knee_idx, p.POSITION_CONTROL, force=100) # 仿真主循环 t = 0.0 while True: # 期望角度：髋关节做正弦摆动，膝关节随动 hip_target = 0.4 * math.sin(t * 3.0) knee_target = -0.8 + 0.3 * math.sin(t * 3.0 + 0.5) p.setJointMotorControl2( robot, hip_idx, p.POSITION_CONTROL, targetPosition=hip_target, force=50.0 ) p.setJointMotorControl2( robot, knee_idx, p.POSITION_CONTROL, targetPosition=knee_target, force=50.0 ) p.stepSimulation() time.sleep(1.0 / 240.0) t += 1.0 / 240.0 if __name__ == "__main__": main()

这段代码的作用是：

• 连接 PyBullet GUI。
• 加载地面和单腿机器人。
• 使用POSITION_CONTROL模式让髋关节和膝关节按照正弦轨迹摆动。
• 以 240 Hz 频率推进物理仿真。

运行方式：

cd quadruped_robot python scripts/single_leg_demo.py

预期结果：窗口打开后，机器人基座固定在空中，大腿和小腿按周期摆动，不会摔倒。

4.4 逆运动学求解器

单腿摆动演示只能看到“关节角变化”，如果希望足端走出特定轨迹，就需要逆运动学。

在scripts/ik_solver.py中写入以下二维几何法求解函数：

import math def solve_leg_ik(x, z, L1=0.4, L2=0.35): """ 二维两连杆腿逆运动学。 输入： x : 足端相对髋关节的前向位置 z : 足端相对髋关节的垂直位置，向下为负 L1 : 大腿长度 L2 : 小腿长度 输出： theta_hip : 髋关节角度 theta_knee : 膝关节角度 """ r = math.hypot(x, z) # 检查可达性 if r > L1 + L2 or r < abs(L1 - L2): raise ValueError("目标位置超出工作空间") cos_knee = (r * r - L1 * L1 - L2 * L2) / (2 * L1 * L2) cos_knee = max(-1.0, min(1.0, cos_knee)) # 小腿相对大腿的夹角（负值） theta_knee = -math.acos(cos_knee) alpha = math.atan2(x, -z) beta = math.acos((L1 * L1 + r * r - L2 * L2) / (2 * L1 * r)) theta_hip = alpha - beta return theta_hip, theta_knee

这是一个常见的二维逆运动学解法。需要注意的是，不同模型关节正方向可能不同，大家要根据自己的 URDF 中关节 axis 方向和初始姿态调整符号。

4.5 Trot 步态规划核心片段

接下来是四足机器人最关键的步态规划。在scripts/gait_planner.py中，我给出一个 Trot 步态的相位判断和足端轨迹生成代码：

import math def get_phase(time_t, cycle_time=0.6, offset=0.0): """ 计算某条腿在步态周期中的相位。 offset 用于错开不同腿的相位。 """ phase_ratio = ((time_t / cycle_time) + offset) % 1.0 return phase_ratio def compute_swing_stance(phase_ratio, step_length=0.1, step_height=0.05): """ 根据相位计算足端相对髋关节的期望位置偏移。 phase_ratio < 0.5 为摆动相，否则为支撑相。 """ if phase_ratio < 0.5: # 摆动相：抬腿并向前迈步 s = phase_ratio / 0.5 x_offset = step_length * (s - 0.5) z_offset = -step_height * math.sin(math.pi * s) else: # 支撑相：足端相对向后移动，推动身体前进 s = (phase_ratio - 0.5) / 0.5 x_offset = step_length * (0.5 - s) z_offset = 0.0 return x_offset, z_offset def get_leg_command(time_t, leg_offset, cycle_time=0.6): """ 得到单条腿的期望足端偏移。 leg_offset 取值为 0, 0.5, 0.5, 0，用于实现对角腿同步。 """ phase_ratio = get_phase(time_t, cycle_time, leg_offset) x_offset, z_offset = compute_swing_stance(phase_ratio) return x_offset, z_offset

在这个设计中，四条腿的 offset 分别取：

• 左前腿：0
• 右后腿：0
• 右前腿：0.5
• 左后腿：0.5

这样可以形成“左前 + 右后”同步摆动，“右前 + 左后”同步摆动的 Trot 步态。

4.6 将步态规划接入仿真主循环

假设你已经有一个完整四足机器人 URDF，四条腿的髋关节和膝关节索引分别对应[hip_LF, knee_LF, hip_RF, knee_RF, hip_LH, knee_LH, hip_RH, knee_RH]，主循环大致如下：

for t in range(int(10 * 240)): sim_time = t / 240.0 # 每条腿的相位偏移 leg_offsets = { "LF": 0.0, "RF": 0.5, "LH": 0.5, "RH": 0.0, } # 计算每条腿的目标足端位置 target_xyz = {} for leg_name, offset in leg_offsets.items(): x_off, z_off = get_leg_command(sim_time, offset) # 这里的 base_x / base_z 是基本足端位置 target_xyz[leg_name] = (base_x + x_off, base_z + z_off) # 使用逆运动学解算关节角度 for leg_name, (x, z) in target_xyz.items(): hip_angle, knee_angle = solve_leg_ik(x, z) # 设置位置控制目标 ... p.stepSimulation() time.sleep(1 / 240)

这里省略了从腿名到关节索引的映射，因为不同 URDF 的关节顺序差异很大。大家需要根据自己模型的p.getJointInfo()打印结果来匹配。

5. 常见问题与排查思路

5.1 常见报错汇总

5.2 排查完整清单

如果你运行仿真时机器人行为异常，可以按以下顺序排查：

1. 确认 URDF 能正常显示，所有连杆和关节是否出现。
2. 打印关节索引和关节名称，确认控制的是否是正确的关节。
3. 测试单个关节，给定固定角度，看腿是否移动到期望位置。
4. 加入逆运动学后，先静态设定一个目标点，确认 IK 求解结果是否稳定。
5. 最后再切入动态步态循环，逐条腿增加摆动，观察互相影响。

5.3 如何避免再次出现

• 每个关节都设置合理的lower和upper限位，避免动作超出机械极限。
• 仿真步长固定为1/240，不要在主循环中随意调整time.sleep的数值。
• 逆运动学结果要做可达性检查和限位截断。
• 控制频率尽量高于 100 Hz，否则高速运动时会出现明显滞后。

6. 最佳实践与工程建议

6.1 仿真到真机的核心差异

仿真跑通，不代表真机也能直接跑。以下几个差异非常关键：

• 模型误差：仿真中的连杆质量、惯性参数、摩擦系数都是理想值，真机存在加工误差和装配误差，需要标定。
• 通信延迟：真机控制通常要走 CAN、EtherCAT 等总线，指令下发和传感器回传有延迟，控制周期可能下降。
• 电源限制：仿真中的电机力是无穷大的，真机受电池和驱动器限制，电压跌落会导致力矩不足。
• 安全保护：真机必须有限位、急停、过流保护，仿真里这些都不存在。

建议在进入真机前，先在仿真中做“故障注入”，比如模拟突然的落地冲击、单腿失效、传感器噪声，提前验证控制器的鲁棒性。

6.2 控制参数调优建议

PD 控制器的参数不是越大越好。经验做法是：

• 先调大Kp，让关节能跟上期望位置。
• 保持Kp不变，逐渐增大Kd，消除抖动。
• 如果出现“软绵绵”跟不上轨迹，说明Kp太小；如果出现高频抖动，说明Kd不足或仿真步长太大。
• 在 PyBullet 中，可以把Kp和Kd作为命令行参数，方便批量实验。

6.3 代码组织与工程化管理

机器人控制代码会随着功能增加快速膨胀，建议从一开始就做好分层：

sensor_layer：读取关节状态、IMU、足端力传感器 perception_layer：处理视觉、激光雷达数据 planning_layer：步态规划、路径规划 control_layer：关节控制、全身运动控制 hardware_layer：与真机硬件通信

这种分层的好处是，仿真和真机可以共用上层算法，只需要替换底层的硬件接口层。

6.4 日志与可视化

在开发和调试阶段，建议把关节角、角速度、足端期望位置、实际位置等数据记录下来。可以保存成 CSV，后续用 Python 或 MATLAB 绘制曲线，观察轨迹跟踪效果。

PyBullet 也支持添加调试参数和调试线段，比如：

p.addUserDebugParameter("hip_target", -1.5, 1.5, 0.0) p.addUserDebugLine(startPos, endPos, [1, 0, 0])

这样可以在 GUI 中实时拖动参数，快速观察机器人行为。

6.5 生产环境的安全注意事项

涉及真机调试时，以下安全规则必须遵守：

• 先在仿真中跑通所有场景，再上真机。
• 真机实验前，支起安全绳或用吊架支撑机器人。
• 设置关节力矩上限，启动急停功能。
• 操作人员保持安全距离，避免被机器人踢伤。
• 代码上线前，同事之间做 code review，尤其是关节限位和安全判断逻辑。

7. 总结与学习路线

这篇文章从“机器人跑得快”的现象出发，介绍了四足机器人运动控制的基本概念，并基于 PyBullet 完成了从单腿摆动到 Trot 步态规划的完整示例。你可以从中掌握：

• URDF 模型的基本结构。
• PyBullet 关节控制方式和主循环写法。
• 正运动学、逆运动学的基本建模思路。
• Trot 步态的相位分配和足端轨迹生成方法。
• 常见仿真异常排查与工程化建议。

接下来如果继续深入学习，建议按这个顺序推进：

1. 先在 PyBullet 中搭建完整四足机器人，把四条腿的 Trot 步态跑起来。
2. 加入 IMU 反馈和姿态控制，让机器人具备抗干扰能力。
3. 学习动力学控制，理解 Swing Leg 的力矩补偿和 Stance Leg 的柔顺控制。
4. 了解 ROS 2 的机器人开发流程，把控制节点与感知、规划模块对接。
5. 有条件时，尝试把仿真算法部署到开源四足硬件平台上。

机器人运动控制的学习曲线并不平缓，但每跑通一个实验，你对“机器人是怎么动起来”的理解都会更深一层。建议先动手把单腿摆动跑起来，再从一条腿扩展到四条腿，慢慢感受步态切换的微妙之处。