用Python+NumPy手把手实现四足机器人腿部三维运动学（附完整代码与避坑点）

用Python+NumPy手把手实现四足机器人腿部三维运动学（附完整代码与避坑点）

四足机器人的运动控制一直是机器人学中最具挑战性的领域之一。想象一下，当你看到一只机械狗灵活地穿越复杂地形时，背后其实是数百行精密的运动学代码在实时计算每个关节的角度。本文将带你从零开始，用Python和NumPy实现四足机器人腿部的三维运动学模型，让你不仅能理解背后的数学原理，更能获得可直接用于实际项目的代码模块。

1. 理解四足机器人腿部的基本结构

四足机器人的单腿通常由3-4个旋转关节组成，形成类似动物腿部的结构。我们以最常见的三关节配置为例：

• 髋关节（θ₀）：控制腿部在垂直平面内的摆动
• 大腿关节（θ₁）：控制腿部的前后运动
• 小腿关节（θ₂）：完成步态的精细调整

这种结构在机器人学中被称为"3DOF串联机构"，其运动学模型需要考虑三个维度的坐标变换。下面是一个典型的四足机器人腿部参数表：

提示：在实际机器人设计中，这些参数需要根据机器人的尺寸和重量分布进行优化，本文提供的代码允许你自由调整这些参数。

2. 建立三维运动学模型

2.1 坐标系定义

我们需要建立三个坐标系来描述腿部运动：

1. 基坐标系：固定在机器人躯干中心
2. 髋关节坐标系：随髋关节旋转
3. 足端坐标系：描述足部位置

import numpy as np def create_rotation_matrix(theta, axis='z'): """创建绕指定轴旋转的3x3矩阵""" c, s = np.cos(theta), np.sin(theta) if axis == 'x': return np.array([[1, 0, 0], [0, c, -s], [0, s, c]]) elif axis == 'y': return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]]) elif axis == 'z': return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])

2.2 正运动学实现

正运动学解决的是"已知关节角度，求足端位置"的问题。我们需要按顺序计算每个关节的变换：

def forward_kinematics(theta0, theta1, theta2, a, L1, L2): """计算足端位置(x,y,z)""" # 髋关节变换 R0 = create_rotation_matrix(theta0, 'x') p0 = np.array([0, a, 0]) # 大腿关节变换 R1 = create_rotation_matrix(theta1, 'y') p1 = np.array([0, 0, 0]) # 小腿关节变换 R2 = create_rotation_matrix(theta2, 'y') p2 = np.array([L1, 0, 0]) # 足端位置 p_foot = np.array([L2, 0, 0]) # 组合变换 T = (R0 @ (p0 + R1 @ (p1 + R2 @ (p2 + p_foot)))) return T

注意：在实际编码中，我们使用@运算符进行矩阵乘法，这比np.dot()更直观且易于阅读。

3. 逆运动学求解

逆运动学是运动控制中的核心难题——"给定足端位置，求解关节角度"。我们需要处理非线性方程组和多解问题。

3.1 几何法求解

对于我们的三关节腿部结构，可以采用分步几何解法：

1. 首先求解髋关节角度θ₀
2. 然后在大腿-小腿平面内求解θ₁和θ₂

def inverse_kinematics(x, y, z, a, L1, L2): """计算关节角度(theta0, theta1, theta2)""" # 处理奇异点 if np.isclose(x, 0) and np.isclose(y, 0): raise ValueError("Singularity: foot directly under hip") # 计算θ0 H_squared = y**2 + z**2 - a**2 if H_squared < 0: raise ValueError("Target position unreachable") H = np.sqrt(H_squared) theta0 = np.arctan2(H, a) - np.arctan2(np.abs(z), y) # 计算θ2 c2 = (x**2 + H**2 - L1**2 - L2**2) / (2 * L1 * L2) # 处理数值误差 c2 = np.clip(c2, -1.0, 1.0) s2 = np.sqrt(1 - c2**2) theta2 = np.arctan2(s2, c2) # 计算θ1 k1 = L1 + L2 * c2 k2 = L2 * s2 theta1 = np.arctan2(H, x) - np.arctan2(k2, k1) return theta0, theta1, theta2

3.2 处理奇异点

四足机器人的腿部运动存在两种主要奇异点：

1. 髋关节奇异点：当足端直接位于髋关节正下方时
2. 伸展奇异点：当大腿和小腿完全伸直或完全折叠时

def is_singular_position(theta1, theta2): """检查是否接近奇异位置""" # 完全伸直 if np.isclose(theta1 + theta2, 0): return True # 完全折叠 if np.isclose(theta1 + theta2, np.pi): return True return False

4. 完整代码实现与测试

现在我们将所有功能整合到一个Python类中，方便在实际项目中使用：

class QuadrupedLegKinematics: def __init__(self, a=80, L1=180, L2=180): self.a = a # 髋关节偏移 self.L1 = L1 # 大腿长度 self.L2 = L2 # 小腿长度 def forward(self, theta0, theta1, theta2): """正运动学""" # ... (同前文forward_kinematics实现) def inverse(self, x, y, z): """逆运动学""" # ... (同前文inverse_kinematics实现) def check_reachable(self, x, y, z): """检查目标位置是否可达""" try: self.inverse(x, y, z) return True except ValueError: return False def visualize(self, theta0, theta1, theta2): """简单的ASCII可视化""" # 实现简单的终端可视化 print(f"Leg Position: θ0={np.degrees(theta0):.1f}°") print(f" θ1={np.degrees(theta1):.1f}°") print(f" θ2={np.degrees(theta2):.1f}°")

4.1 单元测试

为确保代码正确性，我们应该编写测试用例：

def test_kinematics(): """测试运动学模型的正确性""" leg = QuadrupedLegKinematics(a=80, L1=180, L2=180) # 测试正运动学->逆运动学闭环 test_angles = [ (0, np.pi/4, -np.pi/4), # 正常位置 (0.2, 0.3, -0.5), # 随机角度 (np.pi/6, np.pi/3, -np.pi/6) # 较大角度 ] for angles in test_angles: x, y, z = leg.forward(*angles) computed_angles = leg.inverse(x, y, z) assert np.allclose(angles, computed_angles, atol=1e-6) # 测试奇异点检测 assert leg.check_reachable(0, 100, 0) is False print("All tests passed!")

5. 实际应用中的关键技巧

5.1 角度平滑处理

在实际控制中，突然的角度变化会导致电机过载，我们需要对角度进行平滑：

def smooth_angles(current, target, max_step=np.radians(5)): """限制角度变化率""" delta = target - current delta = np.clip(delta, -max_step, max_step) return current + delta

5.2 步态规划基础

一个简单的三角步态生成器可以这样实现：

class GaitGenerator: def __init__(self, stride_length=100, leg_height=50): self.stride = stride_length self.height = leg_height def get_foot_position(self, phase, leg_index): """根据步态相位计算足端位置""" # 相位范围0-1 x = -self.stride/2 + self.stride * phase z = -self.height * np.sin(phase * np.pi) if phase < 0.5 else 0 y = 100 # 侧向固定位置 return x, y, z

5.3 性能优化技巧

对于需要高频计算的应用，我们可以使用Numba加速：

from numba import njit @njit def fast_atan2(y, x): """优化版的atan2函数""" # Numba优化的实现...

在机器人控制中，运动学计算往往需要每秒数百次的执行，这些优化可以显著提升系统响应速度。