3、机械臂运动学基础（下）：正运动学求解、逆运动学求解（解析法与数值法）
好，咱们接着聊。上一节我们把机械臂的坐标系和DH参数搞定了，那都是“地基”。这一节，咱们要在这地基上盖房子了——正运动学和逆运动学。
说白了，正运动学就是：我知道每个关节转了多少度，你给我算出末端执行器在哪儿、朝哪个方向。逆运动学正好反过来：我想让末端去某个位置、摆某个姿态，你告诉我每个关节该转多少度。
听起来好像逆运动学更实用对吧？没错，实际编程里我们天天跟逆解打交道。但正解是基础，不理解正解，逆解你根本玩不转。
核心一句话：正解是唯一的，逆解可能有多个、一个、甚至没有。
3.1 正运动学求解——从关节空间到笛卡尔空间
正运动学，其实就是矩阵连乘。你想想看，每个关节我都用一个齐次变换矩阵来描述它相对于前一个关节的位置和姿态。从基座开始，一路乘到末端，得到的就是末端在基座坐标系下的位姿。
公式长这样：
T_06 = T_01 * T_12 * T_23 * T_34 * T_45 * T_56这里的 T_ij 就是第 j 个关节相对于第 i 个关节的变换矩阵。每个 T_ij 里都包含了 DH 参数：θ、d、a、α。
我记得刚入行那会儿，自己手算过一个六轴机器人的正解。六个矩阵乘下来，光展开就是几十项三角函数。算完之后眼睛都花了。后来我学乖了，直接用 MATLAB 或者 Python 的符号计算库来搞。
这里给一段我常用的 Python 代码片段，用 SymPy 做符号推导：
import sympy as sp
# 定义关节变量
theta1, theta2, theta3, theta4, theta5, theta6 = sp.symbols('theta1:7')
# DH参数表（示例：典型的6R工业机器人）
# [theta, d, a, alpha]
dh_params = [
[theta1, 0.3, 0.05, sp.pi/2],
[theta2, 0, 0.4, 0],
[theta3, 0, 0.05, sp.pi/2],
[theta4, 0.4, 0, -sp.pi/2],
[theta5, 0, 0, sp.pi/2],
[theta6, 0.1, 0, 0]
]
def dh_transform(theta, d, a, alpha):
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 = sp.eye(4)
for params in dh_params:
T = T * dh_transform(*params)
# 简化结果
T_simplified = sp.simplify(T)
print(T_simplified)这段代码跑完之后，你会得到一个 4x4 的矩阵。矩阵的前 3x3 是姿态（旋转矩阵），右上角的 3x1 是位置（x, y, z）。
我的小技巧：实际项目中，我不会每次都做符号推导。我会把正解写成数值函数，输入六个角度，直接输出位姿。这样在离线编程里调用起来飞快。
3.2 逆运动学求解——解析法
逆运动学才是真正的硬骨头。为什么？因为正解是“一个萝卜一个坑”，逆解是“一个萝卜可能有好几个坑”。
解析法，也叫封闭解法。它的核心思想是：利用机械臂的几何结构，通过代数运算直接解出关节角度的表达式。
什么样的机械臂能用解析法？
- 存在腕部偏置：比如最后三个关节的轴线交于一点（球形腕）。这是最常见的。
- 相邻关节轴线平行或垂直：这样很多项会消掉，方程变得可解。
我记得有一次，我调试一台协作机器人，它的腕部不是球形腕，有偏置。我试图用解析法去解，结果折腾了两天，推导出来的公式有十几页纸。最后我放弃了，改用数值法。嗯，这里要注意：不是所有机器人都适合解析法。
解析法的典型步骤（以球形腕机器人为例）：
- 分离位置和姿态：前三个关节决定腕部中心的位置，后三个关节决定末端姿态。
- 求解前三个关节：通过腕部中心的位置，用几何法解出 θ1、θ2、θ3。
- 求解后三个关节：利用末端姿态和已知的前三个关节，解出 θ4、θ5、θ6。
举个例子，求 θ1 的时候，通常是这样：
# 已知腕部中心位置 (px, py, pz)
# 以及 DH 参数中的 d1, a1, a2 等
# θ1 有两个可能解
theta1_1 = sp.atan2(py, px)
theta1_2 = sp.atan2(-py, -px) # 另一个解你看，光是第一个关节就有两个解。整个六轴机器人，理论上最多可以有 8 组逆解。实际中因为关节限位，可能只有 2-4 组是可达的。
避坑指南：我曾经在项目里直接用了第一组逆解，结果机器人撞到了旁边的工装。后来我才意识到，逆解的选择要考虑关节限位、避障、以及运动连续性。千万别无脑选第一个！
3.3 逆运动学求解——数值法
数值法，说白了就是“猜+迭代”。我给它一个初始猜测，然后不断调整关节角度，让末端位姿越来越接近目标位姿。
最常用的数值法是 牛顿-拉夫森法 和 雅可比伪逆法。
核心思路：
- 定义误差函数 e = T_current - T_target
- 利用雅可比矩阵 J，把关节速度映射到末端速度
- 每次迭代：Δθ = J⁻¹ * e（或者用伪逆）
- 更新 θ = θ + Δθ，直到误差小于阈值
代码实现大概长这样：
import numpy as np
def inverse_kinematics_numerical(robot, target_pose, initial_guess, max_iter=100, tol=1e-6):
theta = initial_guess.copy()
for i in range(max_iter):
current_pose = robot.forward_kinematics(theta)
error = target_pose - current_pose # 位置和姿态误差
if np.linalg.norm(error) < tol:
return theta
J = robot.jacobian(theta) # 计算雅可比矩阵
J_pinv = np.linalg.pinv(J) # 伪逆
delta_theta = J_pinv @ error
theta += delta_theta
raise Exception("未收敛，请检查初始猜测或目标位姿是否可达")数值法的好处是通用——不管你的机器人结构多奇葩，都能算。坏处是：
- 需要好的初始猜测，否则可能收敛到奇异点或者根本不收敛
- 每次迭代都要算正解和雅可比，计算量大
- 不能保证找到所有解
我的经验：在实际的离线编程软件里，我通常先用解析法快速算一遍，如果解析法失效（比如遇到奇异位形），再 fallback 到数值法。这样既快又稳。
3.4 解析法 vs 数值法——怎么选？
我整理了一个对比表，你一看就明白：
| 对比项 | 解析法 | 数值法 |
|---|---|---|
| 计算速度 | 极快（微秒级） | 较慢（毫秒级） |
| 解的完整性 | 能找到所有解 | 只能找到一个解 |
| 通用性 | 依赖机械臂结构 | 通用，任意结构 |
| 实现难度 | 高（需要数学推导） | 中等（需要数值计算库） |
| 奇异点处理 | 需要单独处理 | 伪逆可以自然处理 |
我个人习惯是：能解析就解析，不行再数值。尤其是在做轨迹规划的时候，需要连续多帧求解逆解，解析法的速度优势太明显了。
3.5 本章知识体系
下面这张图，是我自己画的，把这一节的核心逻辑串起来了：
这张图把正解和逆解的关系、以及两种逆解方法的优缺点都画清楚了。你把它存下来，以后写代码的时候对照着看，思路会清晰很多。
最后说一句：运动学是机械臂编程的“内功”。内功练好了，后面学轨迹规划、碰撞检测、离线编程，都会事半功倍。别急着跑，先把正解逆解吃透。
公众号:蓝海资料掘金营，微信deep3321