机器人逆运动学（IK）到底在算什么？五个关键点从数学本质到工程落地

大家好，这里是【深蓝具身智能】。

本文出自《具身智能基础》专栏，是本栏目下的第九篇文章，聚焦于逆运动学。

全文 5000 余字，建议收藏阅读。

问：机械臂需要把一颗螺丝精确拧入一个斜面上的孔位，目标位置和姿态已知，每个关节该转多少度？

这个问题，从直觉上看似乎只是"倒着算一遍"，但真正推导下来就会发现它完全不是这么回事。

• 正向的计算（已知关节角→求末端位姿）是一条单行道：

给定输入，通过确定性的矩阵连乘，输出唯一结果。整个过程没有歧义，数值稳定，计算量可控。

• 逆向的问题（已知末端位姿→求关节角）则像是在迷宫里找出口：

出口可能有一个、八个、无数个，甚至根本不存在。更麻烦的是，就算出口存在，找到它的路径本身就是一个非线性的高维搜索问题，在某些特殊位置，搜索的"地图"会直接失效。

这就是逆运动学（Inverse Kinematics，IK）：机器人控制栈里那个承受着数学压力、工程约束和实时性要求三重夹击的核心模块。

本文尝试从数学本质出发，走完解析解→数值解→现代方法→工程落地这条完整的链路，并在关键节点给出可执行的伪代码和核心公式。

位姿的数学空间：为什么 IK 天然就难

末端位姿活在 SE(3) 上

机械臂末端的位姿（Pose）由位置和姿态两部分组成，数学上它是特殊欧氏群 SE(3) 的一个元素：

SE(3) 是一个李群，不是平坦的欧几里得空间。（在专栏的第一篇文章中有详细介绍）

这意味着你不能直接对两个位姿做线性插值——两个旋转矩阵的平均不是旋转矩阵。

这个事实在后续的误差计算和轨迹插值中会反复造成麻烦。

正运动学：确定性的链式计算

正运动学（Forward Kinematics, FK） 通过 DH 参数（Denavit-Hartenberg Convention）将 n 个关节的变换依次连乘：

难点在于：

• f 是非线性的（含大量三角函数）

• 解可能不存在（目标在工作空间外）

• 解可能不唯一（6DOF 球腕机器人最多有 16 组解析解）

• 解可能无穷多（冗余机器人，n > 6）

• 在奇异点附近，问题的数值条件急剧恶化

解析解（Analytical IK）：能算就别迭代

DH 参数与连杆变换

满足 Pieper 条件（后三轴共点）的机器人存在封闭形式解析解。每个连杆的 DH 变换矩阵为：

字母含义（DH 四参数）：

解析 IK 的伪代码骨架（以6DOF球腕机器人为例）

function analytical_IK(T_target, dh_params) -> list[q]:    solutions = []     # Step 1: 解腕心位置（Wrist Center Position）    # 利用球腕结构，末端姿态决定腕心在哪里    p_wc = T_target[:3, 3] - d6 * T_target[:3, 2]    # p_wc: 腕心在基坐标系中的位置    # d6:   第6连杆偏距（末端法兰到腕心距离）    # T_target[:3,2]: 目标姿态的z轴方向（末端接近方向）     # Step 2: 由腕心位置解 θ1, θ2, θ3（位置关节）    theta1_candidates = solve_theta1(p_wc)          # 通常有2解（前/后）    for theta1 in theta1_candidates:        theta3_candidates = solve_theta3(p_wc, theta1)  # 肘上/肘下，2解        for theta3 in theta3_candidates:            theta2 = solve_theta2(p_wc, theta1, theta3)             # Step 3: 构造前三轴的变换 T_03            T_03 = fk(dh_params[:3], [theta1, theta2, theta3])             # Step 4: 由目标姿态与 T_03 之差，解 θ4, θ5, θ6（姿态关节）            R_36 = T_03[:3,:3].T @ T_target[:3,:3]            theta4, theta5, theta6 = euler_ZYZ_from_R(R_36)            # R_36: 第3到第6坐标系间的旋转，用欧拉角分解             solutions.append([theta1, theta2, theta3,                               theta4, theta5, theta6])     # Step 5: 过滤超出关节限位的解，从合法解中选最优    valid = filter_joint_limits(solutions, q_min, q_max)    return select_best(valid, q_current)  # 最小关节位移原则

数值解（Numerical IK）：雅可比是核心武器

雅可比矩阵：关节速度到末端速度的线性化

字母含义：

基于雅可比的迭代 IK 流程

function jacobian_IK(T_target, q_init, max_iter=100, tol=1e-5):    q = q_init                          # 初始关节角（热启动关键）     for k in range(max_iter):        T_cur = fk(q)                   # 正运动学，得当前末端位姿         # 计算6D误差向量（位置误差 + 旋转误差）        e_pos = T_target[:3,3] - T_cur[:3,3]              # 3D位置误差        R_err = T_target[:3,:3] @ T_cur[:3,:3].T          # 旋转误差矩阵        e_rot = so3_to_axis_angle(R_err)                   # 转为轴角向量        e = concatenate([e_pos, e_rot])  # 6维误差旋量         if norm(e) < tol:            return q                    # 收敛         J = compute_jacobian(q)         # 计算当前构型的雅可比         # ---- 选择求解器 ----        # 方式A：伪逆（冗余机器人，n>6）        dq = J.pinv() @ e         # 方式B：DLS（奇异点附近更稳定）        lambda_sq = adaptive_lambda(J)   # 根据最小奇异值自适应        dq = J.T @ inv(J @ J.T + lambda_sq * I) @ e         # 更新关节角（带步长 α 防止过冲）        alpha = line_search(q, dq, T_target)   # 可选：Armijo线搜索        q = q + alpha * dq        q = clip(q, q_min, q_max)       # 强制关节限位     return None  # 未收敛

阻尼最小二乘（DLS）：奇异点的“救命稻草”

def adaptive_lambda(J, eps=0.01, lambda_max=0.1):    sigma_min = min_singular_value(J)   # SVD最小奇异值    if sigma_min >= eps:        return 0.0                      # 远离奇异点，不加阻尼    else:        # 奇异点附近，平滑增大阻尼        ratio = sigma_min / eps        return lambda_max * (1 - ratio**2)

奇异性：IK 的死穴

三类奇异构型

当  时机器人处于奇异构型，末端在某些方向上瞬间丧失运动能力。

操纵度指标

Yoshikawa（1985）提出用操纵度 w 量化当前构型的灵活性：

U, sigma, Vt = svd(J)# sigma: 各方向的放大因子，最小值趋0 = 即将奇异# 奇异方向 = U中对应最小sigma的列w = prod(sigma)   # 等价于 sqrt(det(J@J.T))

 越大，机器人在当前构型下对末端各方向的控制能力越均衡。

在路径规划阶段把  作为代价函数的一项，可以主动让轨迹绕开奇异区域。

冗余自由度：零空间是"免费的算力"

零空间的数学结构

任务优先级分解（Task Priority Framework）

完整的冗余机器人控制律为：

def redundant_ik_step(q, x_target, g_func):    J = compute_jacobian(q)    J_pinv = pinv(J)     # 主任务：追踪末端位姿    x_cur = fk(q)    dx = compute_error(x_cur, x_target)    # 6D误差旋量    dq_primary = J_pinv @ dx     # 次级任务：在零空间中优化 g(q)    grad_g = numerical_gradient(g_func, q)    null_proj = I - J_pinv @ J    dq_secondary = null_proj @ grad_g     return dq_primary + dq_secondary

场景痛点 × 解决方案 × 关键代码

场景 A：高速工业分拣（实时性第一）

痛点：传送带节拍 ≤ 0.5s，IK 必须在 ≤ 1ms 内给出关节角，还要从多解中选无碰撞最优解。

解决方案：针对固定构型（满足 Pieper 条件）预推解析解，离线生成闭式代码；运行时纯查表+atan2计算，无迭代。

关键代码（IKFAST 风格输出片段）：

def ik_joint1(px, py, pz, d1, a2, a3, d4):    # 由腕心(px,py,pz)解第1关节    # px,py: 腕心在基坐标xy平面的投影    # d1: 机座到第1关节的高度偏置    r = sqrt(px**2 + py**2)     # 腕心在水平面的半径    theta1_up   = atan2(py, px)    theta1_down = atan2(-py, -px)   # 机器人"翻转"构型    return theta1_up, theta1_down def ik_joint3(r, pz, d1, a2, a3, d4):    # 用余弦定理求肘关节角    # r: 腕心水平距离, pz: 腕心高度    D = (r**2 + (pz-d1)**2 - a2**2 - a3**2) / (2*a2*a3)    # D 即 cos(θ3)，|D|>1 则目标超出工作空间    if abs(D) > 1.0:        raise WorkspaceError("Target unreachable")    theta3_elbow_up   = atan2(+sqrt(1 - D**2), D)    theta3_elbow_down = atan2(-sqrt(1 - D**2), D)    return theta3_elbow_up, theta3_elbow_down

场景 B：7轴冗余臂避障（灵活性第一）

痛点：末端路径固定（如沿缝焊接），但中间连杆必须绕开一个动态障碍物。额外的第7个自由度是解决冗余的钥匙，但如何使用它是个优化问题。

解决方案：零空间投影，次级任务 = 最大化机器人与障碍物的最短距离。

关键公式：

def obstacle_avoidance_secondary(q, obstacle_mesh, k0=0.5):    # 计算当前构型到障碍物的最短距离梯度    dq = 1e-5    grad = zeros(n)    for i in range(n):        q_plus = q.copy(); q_plus[i] += dq        q_minus = q.copy(); q_minus[i] -= dq        d_plus  = min_distance_to_obstacle(fk_all_links(q_plus),  obstacle_mesh)        d_minus = min_distance_to_obstacle(fk_all_links(q_minus), obstacle_mesh)        grad[i] = (d_plus - d_minus) / (2 * dq)    return k0 * grad   # 作为 z 向量传入零空间投影

场景 C：灵巧手高自由度 IK（维度爆炸）

痛点：五指灵巧手通常有 20+ 自由度，传统数值 IK 在高维关节空间收敛慢，多解问题更加严重。

训练阶段：  随机采样大量合法关节角 q ~ U(q_min, q_max)  计算对应的 T = fk(q)  用 (q, T) 训练 Normalizing Flow 网络 F：    F: (z ~ N(0,I), T_target) -> q    目标：F(F^{-1}(q | T)) = q（可逆映射，精确似然） 推理阶段：  输入 T_target  从标准正态 z_1, z_2, ..., z_K 采样 K 个噪声  q_i = F(z_i, T_target)    # 并行生成 K 个候选解  过滤 fk(q_i) 距 T_target 误差 > ε 的解  从合法解中选关节位移最小的

• 推理速度：单次前向传播，K=100 个解在 GPU 上约 5ms

• 适用场景：灵巧手抓取规划、人形机器人全身 IK

当灵巧手开展拧瓶盖、USB 插拔等精细装配操作时，受传感器噪声、接触不确定性影响，仅依靠单一逆运动学解鲁棒性较差，需要解集候选池支撑上层重决策。

IKFlow 能够采样多样化逆运动学候选解，天然匹配 “批量生成候选 + 约束择优” 的分层规划范式；

在建模思想层面，该生成式思路与 Diffusion Policy 等基于扩散模型的动作生成框架具备良好兼容性，可构建 “笛卡尔动作规划→多候选 IK 求解→关节轨迹筛选” 的分层控制链路。

工程落地的"脏活"清单

写在最后

正运动学是一道函数题；逆运动学是一道反函数题，而且这个反函数是多值的、不连续的，在某些点上根本不存在。

从 DH 参数到 SE(3) 李群，从解析剥离到雅可比迭代，从阻尼最小二乘到零空间投影，再到 Normalizing Flow——IK 的每一层解法，都是在用不同的数学工具逼近同一件事：

在物理约束和实时性压力下，找到那个让机器人"够到"目标的关节配置。

具身智能时代让 IK 的重要性再次上升：当机器人需要在非结构化环境中完成开放任务，IK 不再只是一个控制模块，而是连接感知、规划和执行的核心接口。

解的质量、速度和鲁棒性，直接决定了整个系统的上限。

正向/逆向运动学、轨迹生成与运动规划等核心模块，覆盖了本专栏正在连载的完整具身智能基础知识体系⬇️

编辑｜小小怪博士

审编｜具身君