3. 运动学基础（下）：齐次变换矩阵、正运动学建模（D-H参数法）、逆运动学求解思路

好，咱们接着聊。上一节我们把旋转矩阵、平移向量这些基本工具捋了一遍。这一节，我们要把它们组合起来，变成真正能用的东西。

说白了，机器人运动学就是回答两个问题：“我的关节动多少，末端到底在哪儿？” 和 “我想让末端去那儿，关节该怎么动？”。前者叫正运动学，后者叫逆运动学。今天我们就来拆解这两个问题。

3.1 齐次变换矩阵：把旋转和平移装进一个“盒子”里

你想想看，一个坐标系相对于另一个坐标系，既有旋转又有平移。如果我们分开算，先旋转再平移，或者先平移再旋转，公式写起来很啰嗦。有没有一种方式，能用一个矩阵搞定？

有，就是齐次变换矩阵。我习惯把它叫做“4x4的瑞士军刀”。

它的形式长这样：

| R   t |
| 0   1 |

其中，R 是 3x3 的旋转矩阵，t 是 3x1 的平移向量。最后一行是 [0 0 0 1]。

这个矩阵的好处是什么？它能把一个点的坐标，从A坐标系变换到B坐标系，只需要一次矩阵乘法。

举个例子：

# 假设有一个点P在A坐标系下是 [1, 2, 3]
# 我们想把它变换到B坐标系下
# 齐次变换矩阵 T_B_A 表示从A到B的变换

import numpy as np

# 定义旋转（绕Z轴转30度）和平移
theta = np.deg2rad(30)
R = np.array([[np.cos(theta), -np.sin(theta), 0],
              [np.sin(theta),  np.cos(theta), 0],
              [0,              0,             1]])
t = np.array([1, 2, 0])

# 构建齐次变换矩阵
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = t

# 点的齐次坐标
p_A = np.array([1, 2, 3, 1])

# 变换
p_B = T @ p_A
print(p_B[:3])  # 输出在B坐标系下的坐标

核心要点：齐次变换矩阵的逆矩阵，就是反向变换。这在串联机器人里非常有用，因为我们要从基座一路变换到末端。

我的小技巧：写代码时，我习惯把齐次变换矩阵封装成一个类。这样代码可读性高，也不容易出错。比如 T = HomogeneousTransform(R, t)，然后直接 T.inverse() 就能得到逆矩阵。

3.2 正运动学建模：D-H参数法

正运动学，就是已知关节角度，求末端位姿。对于人形机器人这种多连杆系统，最经典的建模方法就是 D-H参数法。

D-H参数法，说白了就是给每个关节建立一个坐标系，然后用四个参数来描述相邻坐标系之间的关系。这四个参数是：

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的角度）

嗯，这里要注意：D-H参数有两种约定——标准D-H和改进D-H。 我刚开始学的时候被这个坑过。标准D-H把坐标系建在连杆末端，改进D-H建在连杆起始端。我个人习惯用改进D-H，因为它在处理树形结构（比如人形机器人）时更直观。

有了这四个参数，相邻坐标系之间的齐次变换矩阵就是：

T_i = RotX(α_{i-1}) * TransX(a_{i-1}) * RotZ(θ_i) * TransZ(d_i)

写成矩阵形式：

| cosθ_i   -sinθ_i   0   a_{i-1} |
| sinθ_i*cosα_{i-1}  cosθ_i*cosα_{i-1}  -sinα_{i-1}  -d_i*sinα_{i-1} |
| sinθ_i*sinα_{i-1}  cosθ_i*sinα_{i-1}   cosα_{i-1}   d_i*cosα_{i-1} |
| 0          0          0   1 |

然后，从基座到末端的整体变换矩阵，就是把这些矩阵连乘起来：

T_0_n = T_1 * T_2 * ... * T_n

我曾经踩过的坑：有一次在仿真中，我的机器人腿总是“拧麻花”。查了半天，发现是D-H参数表中，某个连杆的α符号搞反了。记住：α的正负号决定了关节轴的方向，一定要和实际机械结构对应。

3.3 逆运动学求解思路

逆运动学就更有意思了。已知末端位姿，求关节角度。这通常比正运动学难得多。

为什么难？因为：

多解性：同一个末端位姿，可能有多种关节角度组合（比如肘部朝上或朝下）。
奇异性：在某些位姿下，关节速度会突变，甚至无解。
无解：目标位姿超出了机器人的工作空间。

求解方法主要有两类：

3.3.1 解析法（封闭解）

对于某些特定构型的机器人（比如6轴工业机器人，且满足Pieper准则），我们可以通过几何或代数方法直接求出解析解。速度快，精度高。但通用性差，换一个构型就得重新推导。

3.3.2 数值法（迭代法）

最常用的是 雅可比矩阵伪逆法。思路是：

给定当前关节角度 q，计算当前末端位姿 T_current。
计算末端位姿误差 ΔT = T_desired - T_current。
通过雅可比矩阵 J 的伪逆，计算关节角度增量 Δq = J⁺ * ΔT。
更新关节角度 q = q + Δq。
重复直到误差足够小。

代码示意：

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

实战经验：数值法虽然通用，但容易陷入局部最优或遇到奇异点。我一般会结合阻尼最小二乘法（DLS），给雅可比矩阵加一个阻尼因子，让求解更稳定。代码里就是 J_pinv = J.T @ inv(J @ J.T + λ² * I)。

避坑指南：我曾经在一个人形机器人项目中，逆运动学求解总是震荡。后来发现是雅可比矩阵计算有误——我忘了考虑旋转部分的角速度映射。记住：雅可比矩阵要把线速度和角速度都算进去。

3.4 本章知识体系总览

为了让你更直观地理解这一章的知识结构，我画了一张图：

这张图把三个核心模块串起来了。齐次变换矩阵是基础工具，D-H参数法帮我们建立模型，逆运动学则是最终要解决的问题。三者环环相扣，缺一不可。

好了，这一节的内容就到这儿。运动学基础是后续轨迹跟踪、动力学控制的地基。地基打牢了，后面才能盖高楼。

📚 课程目录

3. 运动学基础（下）：齐次变换矩阵、正运动学建模（D-H参数法）、逆运动学求解思路

3.1 齐次变换矩阵：把旋转和平移装进一个“盒子”里

3.2 正运动学建模：D-H参数法

3.3 逆运动学求解思路

3.3.1 解析法（封闭解）

3.3.2 数值法（迭代法）

3.4 本章知识体系总览