3. 运动学基础：正运动学、逆运动学、雅可比矩阵、奇异性分析

运动学，说白了就是研究“怎么动”的学问，不考虑力。你给机器人每个关节一个角度，它末端到底在哪儿？反过来，你想让末端去某个点，每个关节该转多少度？这就是正运动学和逆运动学要干的事。

我刚开始做机器人控制那会儿，觉得这玩意儿就是套公式。后来踩的坑多了才发现——运动学是控制的地基，地基不稳，上层算法再花哨也白搭。尤其是奇异性，嗯，这东西能让你的机器人当场“僵住”。

核心要点：运动学是连接“关节空间”和“笛卡尔空间”的桥梁。正运动学唯一确定，逆运动学多解且复杂，雅可比矩阵描述速度映射，奇异性则是雅可比矩阵的“病态”时刻。

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

正运动学很简单：你给出一组关节角度，我告诉你末端执行器在哪儿、朝哪个方向。这是个一一映射，不存在歧义。

最常用的建模方法是 DH参数法（Denavit-Hartenberg）。我个人习惯用改进DH（Modified DH），因为它在处理树形结构时更直观。你只需要四个参数：

a_i-1：连杆长度，沿X_i-1轴从Z_i-1到Z_i的距离
α_i-1：连杆扭转角，绕X_i-1轴从Z_i-1转到Z_i的角度
d_i：连杆偏距，沿Z_i轴从X_i-1到X_i的距离
θ_i：关节角，绕Z_i轴从X_i-1转到X_i的角度

有了这四个参数，相邻连杆的变换矩阵就出来了。把所有矩阵连乘，就是末端位姿。

我的习惯：写代码时，我会把DH参数表单独做成一个配置文件。这样换机器人型号时，改参数表就行，不用动核心算法。曾经有个项目，就因为DH参数写死在代码里，换臂时改了一整天——后来我学乖了。

举个六轴工业机器人的例子，正运动学计算流程如下：

// 伪代码：六轴机器人正运动学
Matrix4x4 forwardKinematics(float q[6], DHParam dh[6]) {
    Matrix4x4 T = identity();  // 初始为单位矩阵
    for (int i = 0; i < 6; i++) {
        Matrix4x4 Ai = dhTransform(dh[i], q[i]);
        T = T * Ai;  // 连乘
    }
    return T;  // 末端位姿矩阵
}

正运动学计算量不大，实时性很好。在控制周期1ms的系统中，算个几十次都没问题。

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

逆运动学就麻烦多了。你告诉机器人“手伸到那个点”，它得算出每个关节转多少。这是个多解问题——同一个末端位姿，可能有多种关节构型都能达到。

为什么会有多解？你想想看，一个六轴机器人，肘关节可以朝上也可以朝下，腕关节可以翻转也可以不翻。这就是所谓的“肘上/肘下”、“腕翻转/非翻转”等构型。

求解方法主要有两类：

方法	优点	缺点	适用场景
解析法	速度快，解精确，可枚举所有解	需要机器人满足Pieper准则（三个相邻关节轴交于一点）	工业六轴机器人（大多数满足条件）
数值法	通用性强，不依赖机器人结构	迭代慢，可能不收敛，依赖初值	非标准构型、冗余机器人

我在项目中遇到过一个问题：用数值法做逆解，初值给得不好，迭代半天不收敛。后来我改成先用解析法算一个近似解，再用数值法精调——效果好了很多。

避坑指南：我曾经在调试一个协作机器人时，逆运动学算出来的关节角度跳变很大。查了半天，发现是角度归一化没做好。关节角度一般要映射到[-π, π]或[0, 2π)，否则连续运动时角度会“绕圈”。

3.3 雅可比矩阵：速度与力的桥梁

雅可比矩阵 J 描述了关节速度 q̇ 和末端速度 v 之间的关系：

v = J · q̇

它还是个力映射工具：τ = J^T · F，其中τ是关节力矩，F是末端力。说白了，雅可比矩阵就是连接“关节空间”和“笛卡尔空间”的转换器。

雅可比矩阵的维度是 6 × n（n为关节数）。前3行对应线速度，后3行对应角速度。每一列对应一个关节的贡献。

计算雅可比矩阵有两种方式：

解析法：对正运动学方程求偏导。精确但推导繁琐。
几何法：利用运动学链的几何关系直接构造。我更喜欢这种方法，物理意义清晰。

几何法的核心思想：对于旋转关节i，它对末端线速度的贡献是 z_i-1 × (p_n - p_i-1)，对角速度的贡献就是 z_i-1。其中z_i-1是关节i的旋转轴方向，p_n是末端位置，p_i-1是关节i的位置。

实用技巧：在代码实现中，我通常把雅可比矩阵的计算和正运动学放在一起。因为计算雅可比矩阵需要的中间变量（各连杆的位姿）在正运动学里已经算好了，复用即可，避免重复计算。

3.4 奇异性分析：机器人的“死穴”

奇异性，就是雅可比矩阵 J 的秩下降（行列式为0）的那些位形。这时候，机器人在某个方向上失去了运动能力——你让末端往那个方向动，关节速度会趋于无穷大。

为什么会这样？说白了，就是关节的贡献方向“撞车”了。比如两个关节的旋转轴平行了，它们对末端运动的贡献就冗余了。

常见的奇异位形有三种：

类型	描述	典型场景
边界奇异	机器人手臂完全伸直或完全折叠	六轴机器人手臂水平伸直时
内部奇异	两个或多个关节轴共线	腕部关节轴对齐（腕奇异）
结构奇异	并联机构的特殊位形	Stewart平台某些姿态

我记得有一次调试焊接机器人，末端在走圆弧轨迹时突然剧烈抖动。一看日志，关节速度指令飙到了额定值的10倍。原因就是轨迹经过了一个奇异点附近——雅可比矩阵接近奇异，逆解算出来的关节速度爆炸了。

避坑指南：处理奇异性，我常用的策略是“阻尼最小二乘法”（DLS）。在求逆雅可比时加一个阻尼项：J^# = J^T(J·J^T + λ²I)^-1。λ是阻尼系数，奇异时自动增大，牺牲一点精度换取稳定性。这个法子虽然老，但可靠。

另一个实用技巧是“奇异分离法”：把雅可比矩阵做奇异值分解（SVD），找出奇异方向，然后只在该方向上做阻尼处理。这样不影响其他方向的精度。

3.5 小结与工程建议

运动学这块，我给你的建议就三条：

正运动学要快：用DH参数表驱动，代码要写成循环，别手写每个关节的矩阵。
逆运动学要稳：优先用解析法，不行再上数值法。初值选择很关键，可以用上一时刻的解做初值。
奇异性要防：轨迹规划时提前检测奇异区域，要么绕开，要么用阻尼算法硬扛。

嗯，运动学是控制的基础。你把这些搞扎实了，后面讲轨迹规划、动力学控制时，心里就有底了。

📚 课程目录

3. 运动学基础：正运动学、逆运动学、雅可比矩阵、奇异性分析

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

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

3.3 雅可比矩阵：速度与力的桥梁

3.4 奇异性分析：机器人的“死穴”

3.5 小结与工程建议