机械臂运动学：用 Python 快速求解正逆解

在自动化设备与机器人设计中，运动学分析是确保末端执行器精准到达目标位置的基础。传统上，工程师依赖昂贵的商业仿真软件（如RoboDK、MATLAB Robotics Toolbox）。然而，对于定制化需求高、或预算敏感的项目，掌握基于Python的自主分析能力，不仅能省下可观的软件授权费用，更能实现分析流程的深度定制与集成。本文将聚焦于串联机器人，探讨如何利用Python生态完成从建模到求解的全流程。

1. D-H参数法：从物理机构到数学模型

D-H（Denavit-Hartenberg）参数法是描述串联机器人连杆与关节关系的经典方法。其核心思想是用四个参数来定义相邻连杆坐标系间的变换：

**连杆长度 a_i**：沿 X_i 轴，从 Z_i 移动到 Z_{i+1} 的距离。
**连杆扭角 α_i**：绕 X_i 轴，从 Z_i 旋转到 Z_{i+1} 的角度。
**关节距离 d_i**：沿 Z_i 轴，从 X_{i-1} 移动到 X_i 的距离。
**关节转角 θ_i**：绕 Z_i 轴，从 X_{i-1} 旋转到 X_i 的角度。

对于旋转关节，θ_i 是变量；对于移动关节，d_i 是变量。以一个典型的四轴SCARA机器人为例，其D-H参数表可能如下：

关节 `i`	`a_i` (mm)	`α_i` (rad)	`d_i` (mm)	`θ_i` (rad)
1	200	0	0	`θ1` (变量)
2	150	π	0	`θ2` (变量)
3	0	0	`d3` (变量)	0
4	0	0	50	`θ4` (变量)

这张表就是机器人机构的“数字身份证”，是后续所有计算的起点。建立准确的D-H模型，要求工程师对机械结构的空间关系有清晰的理解，这是纯软件操作无法替代的工程经验。

2. 正运动学求解：使用SymPy实现符号推导

正运动学解决“给定各关节角度/位移，求末端位姿”的问题。相邻连杆的变换矩阵由四个参数计算得出。最终末端执行器相对于基坐标系的变换矩阵，是所有连杆变换矩阵的连乘。

这里强烈推荐使用SymPy库进行符号计算。与直接使用NumPy进行数值计算相比，SymPy可以保留 θ1, d3 等符号变量，最终得到一个以关节变量为参数的通用位姿表达式。这对于后续的速度分析、雅可比矩阵求解乃至控制系统设计都至关重要。

import sympy as sp
import numpy as np

# 定义符号变量
theta1, theta2, d3, theta4 = sp.symbols('theta1 theta2 d3 theta4')
# 定义D-H参数（使用上述SCARA示例）
a = [0.2, 0.15, 0, 0]  # 单位：米
alpha = [0, sp.pi, 0, 0]
d = [0, 0, d3, 0.05]
theta = [theta1, theta2, 0, theta4]

def dh_matrix(a, alpha, d, theta):
    """生成标准的D-H变换矩阵"""
    return sp.Matrix([
        [sp.cos(theta), -sp.sin(theta)*sp.cos(alpha), sp.sin(theta)*sp.sin(alpha), a*sp.cos(theta)],
        [sp.sin(theta), sp.cos(theta)*sp.cos(alpha), -sp.cos(theta)*sp.sin(alpha), a*sp.sin(theta)],
        [0, sp.sin(alpha), sp.cos(alpha), d],
        [0, 0, 0, 1]
    ])

# 计算总变换矩阵 T_0_4
T_total = sp.eye(4)
for i in range(4):
    T_total = T_total * dh_matrix(a[i], alpha[i], d[i], theta[i])

# 简化表达式
T_total_simplified = sp.simplify(T_total)
print("末端执行器位姿矩阵（符号形式）：")
sp.pprint(T_total_simplified)

运行上述代码，你将得到一个包含 theta1, theta2, d3, theta4 的4x4齐次变换矩阵。其中前三行三列是旋转矩阵，第四列的前三个元素是末端的位置坐标 [px, py, pz]。将此符号表达式固化后，对于任何一组给定的关节值，只需进行数值代入即可瞬间得到位姿，效率极高。

3. 逆运动学求解：难点与数值策略

逆运动学解决“给定末端目标位姿，求解各关节变量”的问题，这是机器人控制的核心。其难点在于：

解的多重性：同一目标位姿可能对应多组甚至无穷组关节构型（如“左肘”与“右肘”）。
封闭解的存在性：并非所有机构都有解析解。对于六轴以下或特殊构型（如PUMA、SCARA），可通过几何法或代数法求得封闭解。对于更复杂的冗余或特殊机构，可能不存在封闭解。
奇异位形：在奇异点附近，机器人失去某些方向的运动能力，逆解计算会出现病态问题。

当无法求得封闭解时，必须采用数值解法。最常用的是基于雅可比矩阵的迭代法（如牛顿-拉夫森法）。其核心思路是：

计算当前关节构型下的末端位姿 T_current 与目标位姿 T_target 之间的位姿误差。
利用当前位姿的雅可比矩阵 J（表示末端速度与关节速度的线性关系），求解关节角度的增量 Δq = J⁻¹ * Δx。
迭代更新 q = q + Δq，直至误差小于阈值。

# 伪代码示例：数值逆解迭代思路
def numerical_inverse_kinematics(T_target, initial_guess, max_iter=100, tol=1e-6):
    q = np.array(initial_guess)  # 初始关节角度猜测
    for i in range(max_iter):
        T_current = forward_kinematics(q)  # 正运动学计算
        error = pose_error(T_target, T_current) # 计算位姿误差
        if np.linalg.norm(error) < tol:
            return q, True  # 收敛
        J = compute_jacobian(q)  # 计算雅可比矩阵
        # 使用最小二乘法求解 dq = J⁺ * error，避免奇异
        dq = np.linalg.pinv(J) @ error
        q += dq
    return q, False  # 未收敛