📚 课程目录第3章运动学基础

3. 运动学基础

上一节我们把坐标系和DH参数聊透了。这节咱们直接上硬菜——正解、逆解、工作空间。这三个东西，说白了就是机械臂的“我是谁”、“我在哪”、“我能去哪”。

我刚开始带项目时，总觉得逆解是个数学问题，找个库调一下就行。直到有一次，真机在产线上抓了半天空气……嗯，从那以后，我老老实实把运动学从头捋了一遍。

3.1 正运动学求解：从关节到末端

正运动学很简单：给你每个关节的角度，算出末端在空间里的位置和姿态。说白了就是“已知关节，求位姿”。

公式长这样：

T_0_n = T_0_1 * T_1_2 * ... * T_{n-1}_n

每个T就是相邻两个连杆的齐次变换矩阵。把DH参数往里一代，连乘就完事。

我习惯用Python写个通用函数，结构清晰，调试也方便：

import numpy as np

def dh_transform(a, alpha, d, theta):
    ct = np.cos(theta)
    st = np.sin(theta)
    ca = np.cos(alpha)
    sa = np.sin(alpha)
    return np.array([
        [ct, -st*ca,  st*sa, a*ct],
        [st,  ct*ca, -ct*sa, a*st],
        [0,   sa,     ca,    d],
        [0,   0,      0,     1]
    ])

def forward_kinematics(dh_params, joint_angles):
    T = np.eye(4)
    for i, (a, alpha, d, _) in enumerate(dh_params):
        T_i = dh_transform(a, alpha, d, joint_angles[i])
        T = T @ T_i
    return T

小技巧： 调试正解时，先把所有关节置零，看末端是不是在“零位姿态”。如果不对，八成是DH参数符号搞反了。我吃过这个亏。

正解是唯一的。给定一组关节角，末端位姿只有一个结果。这也是后面逆解的基础——你总得知道“目标在哪”，才能反推“关节怎么走”。

3.2 逆运动学求解：从末端到关节

逆解就麻烦多了。给你一个末端位姿，反推关节角度。解可能不存在，可能有一个，也可能有多个。

为什么会这样？因为机械臂是冗余的。比如人的手臂，手放在同一个位置，肘关节可以高可以低。机器人也一样。

逆解方法主要有三种：

方法	原理	适用场景	优缺点
解析法	代数/几何推导闭式解	6轴以下、结构简单	快、精确；但推导复杂，通用性差
数值法	迭代优化（如牛顿-拉夫森）	任意构型	通用；但慢、可能不收敛
学习法	神经网络拟合映射	高自由度、实时性要求低	灵活；但精度依赖训练数据

我个人最常用的是解析法+数值法混合。先用解析法算一个初值，再用数值法精调。这样既快又稳。

这里给一个简单的数值逆解示例，基于雅可比矩阵的伪逆：

def inverse_kinematics_numerical(dh_params, T_target, q_init, max_iter=100, tol=1e-6):
    q = q_init.copy()
    for i in range(max_iter):
        T_current = forward_kinematics(dh_params, q)
        error = se3_log(T_target @ np.linalg.inv(T_current))
        if np.linalg.norm(error) < tol:
            break
        J = compute_jacobian(dh_params, q)
        dq = np.linalg.pinv(J) @ error
        q += dq
    return q

注意： 数值法对初值敏感。初值离真实解太远，容易发散。我建议先用解析法或经验值给个靠谱的初值，再迭代。

我曾经在项目里遇到一个坑：逆解算出来关节角跳变很大，导致电机一顿一顿的。后来发现是没做“多解选择”——同一位置有多个关节组合，应该选离当前关节角最近的那组。

3.3 工作空间分析：机械臂的“活动范围”

工作空间，就是机械臂末端能到达的所有点的集合。说白了，就是它的“地盘”。

分析工作空间，最直接的方法就是蒙特卡洛法——随机采样关节角，算正解，把末端位置画出来。点云密集的地方就是可达区域。

def workspace_monte_carlo(dh_params, num_samples=10000):
    points = []
    for _ in range(num_samples):
        q = np.random.uniform(low=joint_limits[:, 0],
                              high=joint_limits[:, 1])
        T = forward_kinematics(dh_params, q)
        points.append(T[:3, 3])
    return np.array(points)

画出来之后，你能直观看到：

工作空间通常是个“球壳”状，中间有个空洞（因为关节限制）
靠近基座的地方密度高，远离基座的地方密度低
奇异位形附近，可达性变差

  核心结论： 工作空间分析不是为了画个好看的图，而是为了回答三个问题：

目标点能不能到？

到了之后姿态对不对？

路径上会不会撞到奇异点？

下面这张图是我自己项目里常用的工作空间分析流程，帮你把正解、逆解、工作空间串起来：

你看，正解是基础，逆解是工具，工作空间是边界。三者缺一不可。

最后说一句：运动学不是背公式，而是理解“关节空间”和“笛卡尔空间”之间的映射关系。你把这个映射吃透了，后面轨迹规划、动力学、控制，都会顺很多。