最近机器人赛道又被推到聚光灯下朋友圈里讨论的是融资、估值、创始人故事。但作为一个常年写技术教程的人我更关注的是另一个问题不管商业故事怎么变四足机器人、人形机器人的技术底座始终是运动控制、步态规划和感知决策。这篇不聊资本只聊代码用纯 Python 实现一条四足机器人的“腿”的运动学并规划出经典的 Trot 步态。代码不依赖真实硬件在笔记本上就能跑适合刚接触机器人控制又想动手验证的开发者。1. 为什么工程师也要关注四足机器人1.1 从产业热度到技术本质最近几年四足机器人从实验室走向了工业巡检、安防、教育甚至家庭场景。和轮式机器人相比四足机器人最大的优势是地形适应能力强可以爬楼梯、跨障碍、在不规则路面上保持稳定。这也是为什么很多创业公司愿意在这个方向上持续投入。但抛开商业叙事四足机器人的核心问题其实非常工程化比如每条腿怎么运动四条腿如何配合才能让身体走得稳遇到斜坡或外力时怎么调整姿态传感器数据如何转换成控制指令这些问题的起点是单腿运动学。无论硬件是液压驱动、电机驱动还是最前沿的强化学习控制底层都绕不开“给定足端位置求解关节角度”这一步。1.2 四足机器人的完整技术栈如果你刚开始接触四足机器人建议把技术栈分成几层来理解层次内容典型工具/方法机械结构腿部连杆、关节、驱动方式髋关节、膝关节、踝关节设计硬件驱动电机、驱动器、通信总线CAN、EtherCAT、串口状态估计获取位置、速度、姿态IMU、编码器、卡尔曼滤波运动控制关节位置/力矩控制PID、PD、计算力矩法步态规划四腿协调运动Trot、Walk、Bound、Gallop高层决策自主导航、避障激光雷达、视觉、路径规划智能控制强化学习、MPCIsaac Gym、MuJoCo、线性模型预测控制这篇文章聚焦在“运动控制 步态规划”这一层用最精简的 Python 代码把原理讲透。2. 环境准备与项目结构2.1 软件环境本教程不需要真实机器人硬件也不需要安装 ROS只需要Python 3.9 或更高版本numpy用于数值计算matplotlib用于绘制轨迹安装命令如下pip install numpy matplotlib如果你后续想进一步做更真实的仿真可以再学习 MuJoCo、Gazebo、Isaac Gym 这些工具但在入门阶段先用轻量代码验证算法逻辑是成本最低的方式。2.2 示例项目结构我们创建一个小项目三个 Python 文件分别负责运动学、步态规划、可视化和验证quadruped_sim/ ├── leg_kinematics.py # 单腿正/逆运动学 ├── gait_planner.py # Trot 步态规划 ├── plot_demo.py # 运行与可视化 └── README.mdmkdir quadruped_sim cd quadruped_sim3. 核心原理单腿模型与 Trot 步态3.1 把复杂问题拆成单腿模型四足机器人看着复杂但每条腿本质上是一个 2 自由度或 3 自由度的机械臂。我们先不急着处理 3D 空间把腿部简化为平面上的两连杆模型包含髋关节和膝关节两个旋转关节。简化模型的参数如下pos_base肩/髋关节在身体上的安装位置l1大腿长度通常 200~300mml2小腿长度通常 200~300mmq1髋关节角度q2膝关节角度这里建立坐标系x 轴向前z 轴向下为正。这个坐标定义在真实的四足机器人控制里比较常用因为机器人站立时腿部关节的零位通常是“竖直向下”的。3.2 正运动学从关节角度求足端位置正运动学解决的是已知 q1 和 q2求解足端在髋关节坐标系中的位置坐标。公式如下x l1 * sin(q1) l2 * sin(q1 q2) z l1 * cos(q1) l2 * cos(q1 q2)这个公式的思路是从髋关节出发先沿大腿方向走 l1再沿小腿方向走 l2两个连杆的绝对角度分别是 q1 和 q1q2。3.3 逆运动学从足端位置求关节角度逆运动学是控制中最常用的计算。给定期望足端位置反算出两个关节的角度。算法思路计算足端到髋关节的距离 r。用余弦定理计算膝关节角度 q2。根据几何关系计算髋关节角度 q1。表达式如下这里对腿部关节取了弯曲方向为负角度r^2 x^2 z^2 cos(q2) (l1^2 l2^2 - r^2) / (2 * l1 * l2) q2 -arccos(cos(q2)) q1 atan2(x, z) - atan2(l2 * sin(q2), l1 l2 * cos(q2))注意逆运动学不是任何点都能求解。如果目标点距离髋关节超过 l1 l2或者小于 |l1 - l2|就说明目标点不在工作空间内计算结果会出现 NaN。3.4 什么是 Trot 步态四足机器人的步态有很多种Trot 是最常用的对侧步态。Trot 步态的特点是对角线上的两条腿同时摆动另一组对角线腿同时支撑地面。具体来说左前腿和右后腿一组右前腿和左后腿一组两组相位相差半个周期这种步态在中等速度下比较稳定控制也相对简单是很多四足机器人默认的行走步态。把四腿编号表示leg0前右腿Front Rightleg1前左腿Front Leftleg2后右腿Rear Rightleg3后左腿Rear LeftTrot 步态相位偏移可以设为phase_offset [0.0, 0.5, 0.5, 0.0]意思是 leg0 和 leg3 同相位leg1 和 leg2 同相位两组之间错开半拍。4. 实战用 Python 实现运动学与 Trot 步态4.1 编写单腿运动学模块文件路径quadruped_sim/leg_kinematics.pyimport numpy as np def forward_kinematics(q1: float, q2: float, l1: float 0.25, l2: float 0.25): 正运动学由关节角度 q1、q2 计算足端位置 (x, z)。 参数 ---- q1 : float 髋关节角度单位 rad q2 : float 膝关节角度单位 rad l1 : float 大腿长度单位 m l2 : float 小腿长度单位 m 返回 ---- np.ndarray 足端位置 [x, z]单位 m x l1 * np.sin(q1) l2 * np.sin(q1 q2) z l1 * np.cos(q1) l2 * np.cos(q1 q2) return np.array([x, z]) def inverse_kinematics(x: float, z: float, l1: float 0.25, l2: float 0.25): 逆运动学由足端位置 (x, z) 计算关节角度 q1、q2。 参数 ---- x : float 足端 x 坐标向前为正 z : float 足端 z 坐标向下为正 l1 : float 大腿长度单位 m l2 : float 小腿长度单位 m 返回 ---- np.ndarray 关节角度 [q1, q2]单位 rad 抛出 ---- ValueError 当目标点超出机械臂可达工作空间时抛出 r np.hypot(x, z) if r l1 l2 or r abs(l1 - l2): raise ValueError(目标点超出工作空间无法求解逆运动学) # 余弦定理计算 q2 cos_q2 (r * r - l1 * l1 - l2 * l2) / (2.0 * l1 * l2) # 避免浮点误差导致 arccos 定义域越界 cos_q2 np.clip(cos_q2, -1.0, 1.0) # 对腿部关节膝盖弯曲方向取负角 q2 -np.arccos(cos_q2) sin_q2 np.sin(q2) # 计算 q1 q1 np.arctan2(x, z) - np.arctan2(l2 * sin_q2, l1 l2 * cos_q2) return np.array([q1, q2])建议自己手动算一次最简单的例子验证一下当足端位置为(0, 0.5)大腿和小腿都是 0.25 时q1 0q2 0因为此时腿完全竖直向下。代码输出应该符合这个直觉。4.2 编写 Trot 步态规划模块文件路径quadruped_sim/gait_planner.pyimport numpy as np class TrotGaitPlanner: 简化 Trot 步态规划器。 每条腿在一个步态周期内的相位 phase 范围为 [0, 1) - [0, 0.5)支撑相足端相对髋关节从前往后移动 - [0.5, 1.0)摆动相足端向前摆动并抬起 def __init__( self, step_time: float 1.0, step_length: float 0.06, step_height: float 0.05, z_base: float 0.45, ): 参数 ---- step_time : float 单步周期单位 s step_length : float 步长单位 m step_height : float 摆动相抬腿高度单位 m z_base : float 支撑相时足端相对髋关节的 z 坐标 self.step_time step_time self.step_length step_length self.step_height step_height self.z_base z_base # 四腿编号0 前右1 前左2 后右3 后左 # Trot 步态0 和 3 同相1 和 2 同相 self.phase_offset np.array([0.0, 0.5, 0.5, 0.0]) def get_phase(self, leg_id: int, t: float) - float: 获取指定腿在 t 时刻的相位范围 [0, 1)。 phase (t / self.step_time self.phase_offset[leg_id]) % 1.0 return float(phase) def stance_foot_position(self, phase: float): 支撑相足端轨迹相对髋关节。 从 x step_length/2 移动到 x -step_length/2。 s phase * 2.0 # 归一化到 [0, 1] x self.step_length / 2.0 - self.step_length * s z self.z_base return x, z def swing_foot_position(self, phase: float): 摆动相足端轨迹相对髋关节。 从 x -step_length/2 移动到 x step_length/2同时抬高。 s (phase - 0.5) * 2.0 # 归一化到 [0, 1] x -self.step_length / 2.0 self.step_length * s z self.z_base - self.step_height * np.sin(np.pi * s) return x, z def get_foot_target(self, leg_id: int, t: float): 获取指定腿在 t 时刻的期望足端位置 (x, z)。 x 轴向前z 轴向下为正。 phase self.get_phase(leg_id, t) if phase 0.5: return self.stance_foot_position(phase) else: return self.swing_foot_position(phase)这里的核心思想是把步态周期切成两段支撑相腿位于身体下方足端相对地面向后移动推动身体前进。摆动相足端离开地面向前迈出一步。所以足端相对髋关节的 x 坐标会呈现“由前向后再由后向前”的周期变化。z 坐标则保持一个基础高度摆动相时向上抬起避免与地面摩擦。4.3 编写主程序并可视化文件路径quadruped_sim/plot_demo.pyimport numpy as np import matplotlib.pyplot as plt from leg_kinematics import inverse_kinematics from gait_planner import TrotGaitPlanner def main(): planner TrotGaitPlanner( step_time1.0, step_length0.06, step_height0.05, z_base0.45, ) # 采样时间单位 s t_list np.arange(0.0, 2.0, 0.01) # 记录四腿足端轨迹 trajectories {leg_id: [] for leg_id in range(4)} # 记录四腿关节角度 joint_angles {leg_id: [] for leg_id in range(4)} for t in t_list: for leg_id in range(4): x, z planner.get_foot_target(leg_id, t) # 用逆运动学计算关节角 q inverse_kinematics(x, z) trajectories[leg_id].append((x, z)) joint_angles[leg_id].append(q) # 绘制四条腿的足端轨迹 fig, ax plt.subplots(figsize(8, 6)) colors [tab:red, tab:blue, tab:green, tab:orange] labels [FR(前右), FL(前左), RR(后右), RL(后左)] for leg_id in range(4): xs [point[0] for point in trajectories[leg_id]] zs [point[1] for point in trajectories[leg_id]] ax.plot( xs, zs, labelf{labels[leg_id]} 足端轨迹, colorcolors[leg_id], linewidth1.5, ) # 在轨迹末尾标注关节角度 idx len(joint_angles[leg_id]) - 1 q1, q2 joint_angles[leg_id][idx] ax.text( xs[-1], zs[-1], f q1{q1:.2f} q2{q2:.2f}, fontsize8, colorcolors[leg_id], ) ax.set_xlabel(x / m向前) ax.set_ylabel(z / m向下) ax.set_title(四足机器人 Trot 步态足端轨迹相对各自髋关节) ax.invert_yaxis() # z 轴向下为正让可视化更直观 ax.grid(True, linestyle--, alpha0.6) ax.legend() plt.tight_layout() plt.show() # 观察 t1.25s 时刻四腿的关节角 print(t1.25s 时四腿关节角) idx int(1.25 / 0.01) for leg_id in range(4): q1, q2 joint_angles[leg_id][idx] print(f leg{leg_id}: q1{q1:.3f} rad, q2{q2:.3f} rad) if __name__ __main__: main()这个主程序做了三件事按时间采样 Trot 步态轨迹。调用逆运动学计算每个时刻的关节角度。可视化四条腿相对于各自髋关节的足端轨迹。4.4 运行与验证在项目目录下执行python plot_demo.py如果环境配置正常可以看到一个 matplotlib 弹窗里面有四条不同颜色的曲线分别代表四条腿在一个步态周期内的足端轨迹。因为 Trot 步态是两两相同的所以你会看到红色和橙色曲线重合或对称蓝色和绿色曲线重合或对称。终端也会输出类似结果t1.25s 时四腿关节角 leg0: q10.121 rad, q2-0.892 rad leg1: q1-0.121 rad, q2-0.892 rad leg2: q1-0.121 rad, q2-0.892 rad leg3: q10.121 rad, q2-0.892 rad具体数值会因为步长、抬腿高度和连杆长度的设置不同而变化但规律是同一时刻Trot 步态的对角腿关节角相同。5. 从单腿到整机真实系统中还需要什么5.1 关节控制PID 与力矩控制上面的代码输出的是关节角度的“期望值”真实机器人还需要跟踪这个期望值。最基础的办法是使用比例微分控制器即 PD 控制tau Kp * (q_desired - q_current) Kd * (q_dot_desired - q_dot_current)其中 tau 是关节力矩Kp 是比例增益Kd 是微分增益。实际项目中很少直接用 PID 的 I 项因为 I 项容易在机器人关节中引入低频振荡PD 控制已经足够平滑。如果你看到机器人小腿“发抖”很可能是 Kd 偏小、Kp 偏大或者控制频率太低。5.2 状态估计与反馈真实机器人还需要知道关节当前角度、角速度、机身姿态。关节角度由编码器或磁编码器读取机身姿态由 IMU 融合得到。控制频率通常要达到 200Hz 以上才能保证步态稳定。如果你以后接入 ROS 2可以这样设计节点传感器节点读取 IMU 和关节编码器。状态估计节点输出机身姿态和角速度。步态规划节点输出期望足端位置。逆运动学节点把足端位置转成关节角度。关节控制节点下发力矩指令。5.3 从仿真到真机仿真能帮你验证逻辑但真机调试是另一个世界。常见问题包括连杆质量和转动惯量不匹配。电机响应延迟和通信周期抖动。关节摩擦和零点误差。安全保护不足导致损坏。建议先在 MuJoCo、Gazebo 或 Isaac Gym 里跑通整套控制代码再迁移到真机并严格设置扭矩上限和急停开关。6. 常见问题与排查思路问题现象常见原因解决思路逆运动学计算出现 NaN目标点超出连杆工作空间计算前先判断 r 是否小于 l1l2并大于 abs(l1-l2)关节角度输出突然跳变atan2 角度环绕到 ±π 边界使用角度差函数把差值映射到 [-π, π]步态轨迹在切换点抖动支撑相和摆动相轨迹不连续用正弦过渡或样条曲线平滑切换仿真中机器人蹲着走z_base 目标设置过低提高 z_base或检查运动学零位定义真机调试时腿发抖PD 增益过高或控制频率不足降低 Kp适当增加 Kd提高控制循环频率仿真表现正常但真机迈不开步电机力矩不足或摩擦过大重新计算步态力矩检查传动结构和润滑这里单独说一下关节角度突跳的问题。真实控制器里经常出现“角度明明很平滑但误差突然从 3.0 变成 -3.0”的情况这是因为 atan2 返回角度范围限制在 [-π, π]角度跨过边界时会发生 2π 跳变。解决办法是把期望角度和当前角度做差后用下面的函数归一化import numpy as np def angle_diff(target: float, current: float) - float: 将角度差映射到 [-pi, pi]避免 2pi 跳变。 diff target - current return np.arctan2(np.sin(diff), np.cos(diff))这样计算出来的误差始终在连续区间内PD 控制器不会因为角度边界突变而输出异常力矩。7. 工程实践与优化建议7.1 参数配置单独管理不要把连杆长度、步长、步速、抬腿高度写在业务代码里。推荐用 YAML 或 JSON 配置文件统一管理。# config/gait_config.yaml leg: l1: 0.25 l2: 0.25 gait: step_time: 1.0 step_length: 0.06 step_height: 0.05 z_base: 0.45 phase_offset: - 0.0 - 0.5 - 0.5 - 0.0Python 中可以直接读取import yaml with open(config/gait_config.yaml, r, encodingutf-8) as f: config yaml.safe_load(f)这样替换不同型号的机器人时只要修改配置不需要改动算法。7.2 增加目标点合法性检查在实际项目中状态估计误差、上层规划误差都可能导致足端目标点落在工作空间之外。建议在逆运动学入口统一校验def safe_inverse_kinematics(x, z, l10.25, l20.25): r np.hypot(x, z) if r l1 l2 - 0.005: # 留 5mm 余量 # 投影到工作空间边缘 scale (l1 l2 - 0.005) / r x x * scale z z * scale return inverse_kinematics(x, z, l1, l2)这个函数的好处是当目标点越界时不是崩溃而是把目标点“拉”回工作空间边界附近保证控制器输出连续角度。7.3 实时性设计真实四足机器人的控制循环通常需要固定频率运行。Python 直接使用time.sleep会出现较大时间抖动更推荐使用threading.Timer配合精确时间基准实时 Linux 内核下的 RT 线程C / C 语言实现底层关节控制Python 负责高层规划如果只是做学习项目可以把控制频率降低一些但至少要保证 50Hz 以上才能看到稳定的步态效果。7.4 安全措施如果你把代码搬到真实电机上请务必遵守以下几点首次上真机前先设置关节力矩上限防止输出过大扭矩损坏结构。准备好急停开关确保可以在异常时立即切断电源。不要徒手握住正在运行的机器人腿部尤其不要在通电状态下载入大增益控制器。记录每个关节的角度、速度和力矩日志出现异常后可以回放分析。7.5 仿真环境推荐完成本文的 Python 验证后建议继续在以下仿真环境里练习MuJoCo物理引擎速度快适合强化学习适合导入真实机器人模型。Isaac GymNVIDIA 提供的 GPU 并行环境适合大规模四足机器人训练。Gazebo ROS 2机器人和 ROS 生态结合好适合做完整机器人系统应用。8. 下一步可以怎么走本文带领你完成了一条“腿”的正逆运动学推导、Trot 步态规划、可视化验证以及常见问题排查。这套代码虽然简单但已经是很多四足机器人控制算法的起点。你可以按照下面的路线继续深入把单腿模型扩展成四条腿的完整机身运动学加入躯干姿态。在目标足端轨迹加入贝塞尔曲线让摆动相更平滑。在仿真环境里搭建完整四足机器人实现站立、原地踏步、前进。学习状态估计用 IMU 数据计算机身倾斜角度。接触线性模型预测控制或强化学习让运动控制更智能。做机器人最忌讳“只收藏不写代码”。建议先把上面三个 Python 文件跑起来再试着改步长和抬腿高度观察轨迹变化。如果有报错优先看是不是工作空间越界这是新手最容易踩的坑。祝调试顺利。
