机械臂逆运动学（Inverse Kinematics）从入门到实践

一、什么是逆运动学？

在机器人学中，我们经常面临一个基本问题：

已知机械臂末端执行器（手爪/工具）的目标位姿，求解各个关节应该转动多少角度？

这就是逆运动学（Inverse Kinematics, IK）。

与之对应的是正运动学（Forward Kinematics, FK）——已知各关节角度，求末端位姿。

	正运动学 (FK)	逆运动学 (IK)
输入	关节角度 \((\theta_1, \theta_2, \dots, \theta_n)\)	目标末端位姿 \((x, y, z, \alpha, \beta, \gamma)\)
输出	末端位姿	关节角度
难度	唯一解，直接计算	可能无解、多解，需要求解策略

为什么 IK 比 FK 难？

• FK 是唯一确定的映射，IK 可能存在无穷多解（冗余机械臂）、有限多解、甚至无解
• 解的存在性取决于目标点是否在机械臂的工作空间（Workspace）内

---

二、前置知识：正运动学与 DH 参数

2.1 D-H 参数法

Denavit-Hartenberg (DH) 参数是描述机械臂连杆几何关系的经典方法。每个关节用 4 个参数描述：

参数	含义
\(\theta_i\)	绕 \(z_{i-1}\) 轴的旋转角（关节变量）
\(d_i\)	沿 \(z_{i-1}\) 轴的偏移
\(a_i\)	沿 \(x_i\) 轴的连杆长度
\(\alpha_i\)	绕 \(x_i\) 轴的扭转角

相邻两个坐标系之间的齐次变换矩阵为：

\[A_i = \begin{bmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i \\ \sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix} \]

总变换矩阵为各连杆变换的乘积：

\[T = A_1 \cdot A_2 \cdot \dots \cdot A_n \]

2.2 一个简单的二连杆*面机械臂

考虑一个在*面内运动的 2-DOF 机械臂：

       关节2 (θ₂)
      /
  L₂ /
    /
   ●───────── 末端 (x, y)
  /
L₁/
/
● 关节1 (θ₁) —— 基座

正运动学方程：

\[x = L_1\cos\theta_1 + L_2\cos(\theta_1 + \theta_2) \]

\[y = L_1\sin\theta_1 + L_2\sin(\theta_1 + \theta_2) \]

---

三、逆运动学求解方法

3.1 解析法（Analytical / Closed-form）

对于结构简单的机械臂（如 2R *面臂），可以直接推导出解析解。

二连杆臂的 IK 求解：

由正运动学方程，利用几何关系：

\[x^2 + y^2 = L_1^2 + L_2^2 + 2L_1L_2\cos\theta_2 \]

解得：

\[\theta_2 = \pm\arccos\left(\frac{x^2 + y^2 - L_1^2 - L_2^2}{2L_1L_2}\right) \]

注意：\(\theta_2\) 有两个解（肘部朝上 / 肘部朝下），这就是经典的 elbow-up / elbow-down 多解问题。

\[\theta_1 = \arctan2(y, x) - \arctan2(L_2\sin\theta_2, L_1 + L_2\cos\theta_2) \]

优点：计算快、精确
缺点：仅适用于特殊构型（满足 Pieper 准则的 6-DOF 机械臂），通用性差

3.2 数值法——雅可比迭代法（Jacobian Iterative）

对于一般构型的机械臂，最常用的方法是基于雅可比矩阵的数值迭代法。

核心思想

正运动学给出了末端位姿 \(\mathbf{x}\) 和关节角度 \(\mathbf{q}\) 的关系：

\[\mathbf{x} = f(\mathbf{q}) \]

对时间求导得到速度关系：

\[\dot{\mathbf{x}} = J(\mathbf{q}) \cdot \dot{\mathbf{q}} \]

其中 \(J(\mathbf{q})\) 是雅可比矩阵，它描述了关节速度到末端速度的映射。

我们的目标是：已知 \(\dot{\mathbf{x}}\)（希望末端移动的方向和距离），反求 \(\dot{\mathbf{q}}\)：

\[\dot{\mathbf{q}} = J^{-1}(\mathbf{q}) \cdot \dot{\mathbf{x}} \]

但 \(J\) 不一定是方阵（关节自由度 ≠ 末端自由度），所以需要用伪逆：

\[\dot{\mathbf{q}} = J^{\dagger} \cdot \dot{\mathbf{x}} = J^T(JJ^T)^{-1} \cdot \dot{\mathbf{x}} \]

阻尼最小二乘法（Damped Least Squares / Levenberg-Marquardt）

伪逆法在雅可比矩阵奇异（接*奇异位形）时会不稳定，产生极大的关节速度。引入阻尼因子 \(\lambda\)：

\[\dot{\mathbf{q}} = J^T(JJ^T + \lambda^2 I)^{-1} \cdot \dot{\mathbf{x}} \]

这使得在奇异点附*也能*滑求解，是工业级 IK 求解器的常用策略。

---

四、Python 代码实现

在GitHub上有相应的模拟代码可以帮助理解

https://github.com/201905335/Two_Link_RobotArm_imulator

4.1 二连杆臂解析法

import numpy as np
import matplotlib.pyplot as plt

def ik_2link_analytical(x, y, L1, L2, elbow_up=True):
    """
    二连杆*面机械臂解析法 IK
    返回: (theta1, theta2) 弧度制
    """
    # 检查目标是否在工作空间内
    dist = np.sqrt(x**2 + y**2)
    if dist > L1 + L2 or dist < abs(L1 - L2):
        raise ValueError(f"目标点 ({x}, {y}) 超出工作空间!")
    
    # 求 theta2
    cos_theta2 = (x**2 + y**2 - L1**2 - L2**2) / (2 * L1 * L2)
    cos_theta2 = np.clip(cos_theta2, -1, 1)  # 防止数值误差
    
    if elbow_up:
        theta2 = -np.arccos(cos_theta2)  # 肘部朝上
    else:
        theta2 = np.arccos(cos_theta2)   # 肘部朝下
    
    # 求 theta1
    k1 = L1 + L2 * np.cos(theta2)
    k2 = L2 * np.sin(theta2)
    theta1 = np.arctan2(y, x) - np.arctan2(k2, k1)
    
    return theta1, theta2

def fk_2link(theta1, theta2, L1, L2):
    """正运动学：计算各关节和末端位置"""
    x0, y0 = 0, 0
    x1 = L1 * np.cos(theta1)
    y1 = L1 * np.sin(theta1)
    x2 = x1 + L2 * np.cos(theta1 + theta2)
    y2 = y1 + L2 * np.sin(theta1 + theta2)
    return [(x0, y0), (x1, y1), (x2, y2)]

# 测试
L1, L2 = 1.0, 0.8
target_x, target_y = 1.2, 0.6

# 两种解
t1_up, t2_up = ik_2link_analytical(target_x, target_y, L1, L2, elbow_up=True)
t1_down, t2_down = ik_2link_analytical(target_x, target_y, L1, L2, elbow_up=False)

print(f"Elbow-up:   θ1={np.degrees(t1_up):.1f}°, θ2={np.degrees(t2_up):.1f}°")
print(f"Elbow-down: θ1={np.degrees(t1_down):.1f}°, θ2={np.degrees(t2_down):.1f}°")

# 验证
pts = fk_2link(t1_up, t2_up, L1, L2)
print(f"末端位置验证: ({pts[-1][0]:.4f}, {pts[-1][1]:.4f})")

4.2 通用 3-DOF 机械臂数值法（Jacobian 迭代）

import numpy as np

def fk_3dof(q, L):
    """
    3-DOF *面机械臂正运动学
    q: [theta1, theta2, theta3] 关节角度
    L: [L1, L2, L3] 连杆长度
    返回末端 (x, y) 位置
    """
    theta1, theta2, theta3 = q
    x = L[0]*np.cos(theta1) + L[1]*np.cos(theta1+theta2) + L[2]*np.cos(theta1+theta2+theta3)
    y = L[0]*np.sin(theta1) + L[1]*np.sin(theta1+theta2) + L[2]*np.sin(theta1+theta2+theta3)
    return np.array([x, y])

def jacobian_3dof(q, L):
    """数值雅可比矩阵 (2x3)"""
    eps = 1e-6
    J = np.zeros((2, 3))
    f0 = fk_3dof(q, L)
    for i in range(3):
        q_plus = q.copy()
        q_plus[i] += eps
        J[:, i] = (fk_3dof(q_plus, L) - f0) / eps
    return J

def ik_jacobian(target, q_init, L, max_iter=100, tol=1e-4, lam=0.01):
    """
    阻尼最小二乘法 IK 求解
    target: 目标位置 [x, y]
    q_init: 初始关节角度猜测
    L: 连杆长度
    lam: 阻尼因子
    """
    q = np.array(q_init, dtype=float)
    
    for i in range(max_iter):
        pos = fk_3dof(q, L)
        error = np.array(target) - pos
        
        if np.linalg.norm(error) < tol:
            print(f"收敛于第 {i} 次迭代，误差: {np.linalg.norm(error):.6f}")
            return q
        
        J = jacobian_3dof(q, L)
        # 阻尼最小二乘: dq = J^T (JJ^T + λ²I)^{-1} e
        JJT = J @ J.T
        dq = J.T @ np.linalg.solve(JJT + lam**2 * np.eye(2), error)
        q += dq
    
    print(f"达到最大迭代次数，当前误差: {np.linalg.norm(error):.6f}")
    return q

# 测试
L = [1.0, 0.8, 0.5]
target = [1.5, 0.8]
q_init = [0.1, 0.1, 0.1]

solution = ik_jacobian(target, q_init, L)
print(f"\n目标位置: {target}")
print(f"求解角度 (deg): {np.degrees(solution)}")
print(f"正运动学验证: {fk_3dof(solution, L)}")

4.3 可视化

import matplotlib.pyplot as plt
import numpy as np

def plot_arm(q, L, target=None, title=""):
    """可视化机械臂"""
    theta1, theta2, theta3 = q
    joints = [(0, 0)]
    
    x = L[0]*np.cos(theta1)
    y = L[0]*np.sin(theta1)
    joints.append((x, y))
    
    x += L[1]*np.cos(theta1+theta2)
    y += L[1]*np.sin(theta1+theta2)
    joints.append((x, y))
    
    x += L[2]*np.cos(theta1+theta2+theta3)
    y += L[2]*np.sin(theta1+theta2+theta3)
    joints.append((x, y))
    
    xs, ys = zip(*joints)
    
    fig, ax = plt.subplots(1, 1, figsize=(8, 8))
    ax.plot(xs, ys, 'b-o', linewidth=3, markersize=12, label='机械臂')
    ax.plot(0, 0, 'ks', markersize=15, label='基座')
    ax.plot(xs[-1], ys[-1], 'r*', markersize=20, label='末端')
    
    if target:
        ax.plot(target[0], target[1], 'gx', markersize=15, markeredgewidth=3, label='目标点')
    
    ax.set_xlim(-3, 3)
    ax.set_ylim(-3, 3)
    ax.set_aspect('equal')
    ax.grid(True, alpha=0.3)
    ax.legend(fontsize=12)
    ax.set_title(title, fontsize=14)
    plt.show()

# 可视化求解结果
L = [1.0, 0.8, 0.5]
target = [1.5, 0.8]
q_init = [0.1, 0.1, 0.1]
solution = ik_jacobian(target, q_init, L)
plot_arm(solution, L, target=target, title="3-DOF IK 求解结果")

---

五、进阶话题

5.1 冗余机械臂与零空间

当机械臂自由度 > 末端所需自由度时（如 7-DOF 机械臂控制 6-DOF 末端位姿），称为冗余机械臂。

多出的自由度形成零空间（Null Space），可以在不影响末端位置的情况下调整臂形：

\[\dot{\mathbf{q}} = J^{\dagger}\dot{\mathbf{x}} + (I - J^{\dagger}J)\dot{\mathbf{q}}_0 \]

其中 \(\dot{\mathbf{q}}_0\) 是优化目标的梯度（如关节限位避让、避障等）。

5.2 关节限位处理

实际机械臂的关节有角度范围限制，需要在迭代过程中做投影：

q = np.clip(q, q_min, q_max)

5.3 常用 IK 求解库

库	语言	特点
KDL	C++	Orocos 项目，工业级
IKFast	C++/Python	OpenRAVE 生成解析解
PyBullet	Python	内置 IK，支持 URDF
MoveIt	C++/Python	ROS 生态，集成多种求解器
CasADi	C++/Python	数值优化框架

5.4 奇异位形（Singularity）

当雅可比矩阵秩亏时，机械臂处于奇异位形，表现为：

• 某些方向无法移动
• 关节速度趋向无穷大

常见的奇异位形：

• 肩部奇异：腕关节中心位于基座轴线上
• 肘部奇异：肘关节完全伸直或折叠
• 腕部奇异：腕关节的两个轴共线

---

六、总结

                    逆运动学方法
                        │
           ┌────────────┼────────────┐
           │            │            │
        解析法       数值法       优化法
    (封闭解)     (Jacobian)    (非线性规划)
           │            │            │
     结构特殊时    通用方法    复杂约束
     快速精确    迭代收敛     全局搜索

方法	适用场景	速度	精度
解析法	满足 Pieper 准则的 6R 臂	⚡ 极快	✅ 精确
Jacobian 迭代	通用机械臂	🔶 中等	✅ 高
阻尼最小二乘	接*奇异点	🔶 中等	✅ 高
数值优化	复杂约束	🐢 慢	✅ 高

学习建议：

1. 先从二连杆臂的解析解理解 IK 的本质
2. 手写 Jacobian 迭代法体会数值求解的思路
3. 再使用 PyBullet / MoveIt 处理实际项目