4. 运动学与动力学基础：正/逆运动学、雅可比矩阵、动力学建模（拉格朗日法）
各位同学，欢迎来到第四章。这一章，我们要啃一块硬骨头——运动学与动力学。
说实话，很多做机器人控制的朋友，一听到「运动学」三个字就开始头疼。我当年刚入行时也一样，觉得这东西太数学了，离实际代码很远。直到有一次，我调试一个六轴机械臂的抓取动作，发现末端怎么都到不了目标位置……折腾了两天，最后发现是逆运动学解算时选错了分支。嗯，从那以后，我再也不敢轻视运动学了。
这一章，我会尽量用「工程师的直觉」来讲。咱们不搞纯数学推导，而是把每个概念和实际代码、调试经验结合起来。你想想看，搞懂了这些，你就能让机器人「指哪打哪」了。
4.1 正运动学：从关节到末端
正运动学，说白了就是：已知每个关节的角度，求末端执行器的位置和姿态。
举个例子。你有一个两连杆的机械臂，肩关节转了30度，肘关节转了45度。那么手在哪儿？这就是正运动学要回答的问题。
我个人习惯用 DH参数法（Denavit-Hartenberg）来建模。这个方法的核心是：给每个关节建立一个坐标系，然后用四个参数来描述相邻坐标系之间的变换。
- θ（关节角）：绕Z轴的旋转角
- d（连杆偏距）：沿Z轴的平移距离
- a（连杆长度）：沿X轴的平移距离
- α（连杆扭角）：绕X轴的旋转角
有了这四个参数，我们就可以写出相邻关节的齐次变换矩阵。把所有矩阵乘起来，就得到了末端相对于基座的位姿。
代码实现其实很直观。我常用的一个Python示例是这样的：
import numpy as np
def dh_transform(theta, d, a, alpha):
"""计算单个DH参数的齐次变换矩阵"""
ct = np.cos(theta)
st = np.sin(theta)
ca = np.cos(alpha)
sa = np.sin(alpha)
T = np.array([
[ct, -st*ca, st*sa, a*ct],
[st, ct*ca, -ct*sa, a*st],
[0, sa, ca, d ],
[0, 0, 0, 1 ]
])
return T
# 示例：两连杆机械臂
theta1, theta2 = np.radians([30, 45])
d1, d2 = 0, 0
a1, a2 = 1.0, 0.8
alpha1, alpha2 = 0, 0
T01 = dh_transform(theta1, d1, a1, alpha1)
T12 = dh_transform(theta2, d2, a2, alpha2)
T02 = T01 @ T12 # 末端位姿
print("末端位置：", T02[:3, 3])
你看，代码就这么几行。但实际项目中，关节数一多（比如六轴），矩阵乘法就容易出错。我建议你写一个循环，别手写一串矩阵乘法——我曾经吃过这个亏，debug到怀疑人生。
4.2 逆运动学：从末端到关节
逆运动学是正运动学的反向问题：已知末端位姿，求每个关节的角度。
这比正运动学难多了。为什么？因为正运动学有唯一解，而逆运动学可能有多个解、一个解，甚至无解。
你想想看，一个六轴机械臂，末端在同一个位置，手臂可以「弯着」够到，也可以「伸着」够到。这就是多解问题。
逆运动学的求解方法主要有两种：
- 解析法：通过几何关系直接推导出公式。速度快，但只适用于特定构型的机器人（比如有相邻关节轴线相交的情况）。
- 数值法：用迭代逼近的方式求解。通用性强，但计算量大，且可能不收敛。
我个人在实际项目中，更常用数值法，尤其是 雅可比矩阵伪逆法。它的思路是：
- 给定一个初始关节角猜测
- 计算当前末端位姿与目标位姿的误差
- 用雅可比矩阵的伪逆，把误差映射回关节空间，得到关节角的修正量
- 迭代直到误差足够小
代码实现大致如下：
def inverse_kinematics_numerical(target_pose, initial_q, max_iter=100, tol=1e-6):
q = initial_q.copy()
for i in range(max_iter):
# 正运动学：计算当前末端位姿
T_current = forward_kinematics(q)
# 计算位姿误差（位置+姿态）
error = compute_pose_error(target_pose, T_current)
if np.linalg.norm(error) < tol:
break
# 计算雅可比矩阵
J = compute_jacobian(q)
# 伪逆求解关节角增量
dq = np.linalg.pinv(J) @ error
q += dq
return q
这里有个关键点：雅可比矩阵的伪逆。当机器人处于奇异位形时，雅可比矩阵会降秩，伪逆就不稳定了。我建议你在代码里加一个阻尼因子（Damped Least Squares），能有效避免这个问题。
4.3 雅可比矩阵：速度与力的映射
雅可比矩阵，是连接关节空间和操作空间的桥梁。
它有两个核心作用：
- 速度映射：关节速度 → 末端速度
- 力映射：末端力 → 关节力矩
说白了，雅可比矩阵就是「关节运动」和「末端运动」之间的线性变换。你转动关节，末端怎么动？雅可比矩阵告诉你。
它的数学形式是：
v = J(q) · q̇
其中 v 是末端速度（线速度+角速度），q̇ 是关节速度。
计算雅可比矩阵，我习惯用 向量积法。对于旋转关节，雅可比矩阵的每一列可以这样算：
def compute_jacobian(q):
"""计算六轴机械臂的雅可比矩阵（简化版）"""
n = len(q)
J = np.zeros((6, n))
# 计算每个关节的变换矩阵
T = [np.eye(4)]
for i in range(n):
T.append(T[-1] @ dh_transform(q[i], d[i], a[i], alpha[i]))
# 末端位置
p_end = T[-1][:3, 3]
for i in range(n):
# 关节i的Z轴方向（在基坐标系下）
z_i = T[i][:3, 2]
# 关节i到末端的位置向量
p_i = T[i][:3, 3]
r = p_end - p_i
# 线速度部分：z_i × r
J[:3, i] = np.cross(z_i, r)
# 角速度部分：z_i
J[3:, i] = z_i
return J
4.4 动力学建模：拉格朗日法
运动学只关心「位置、速度、加速度」的几何关系，不关心「力」。但机器人要动起来，必须有力。这就是动力学要解决的问题。
动力学建模的方法有很多，我个人最常用的是 拉格朗日法。为什么？因为它基于能量，推导过程相对系统化，不容易出错。
拉格朗日法的核心公式很简单：
L = T - V
其中 L 是拉格朗日量，T 是系统动能，V 是系统势能。
然后，动力学方程由欧拉-拉格朗日方程给出：
d/dt (∂L/∂q̇) - ∂L/∂q = τ
其中 τ 是关节驱动力矩。
看着有点抽象？咱们拿一个最简单的单摆来举例。
单摆的动能 T = ½ m l² θ̇²，势能 V = m g l (1 - cosθ)。
代入拉格朗日方程，得到：
m l² θ̈ + m g l sinθ = τ
这就是单摆的动力学方程。你看，推导过程很清晰。
对于多关节机器人，拉格朗日法会得到一个更通用的形式：
M(q) q̈ + C(q, q̇) q̇ + G(q) = τ
其中：
- M(q)：惯性矩阵，描述质量分布
- C(q, q̇)：科里奥利力和离心力矩阵
- G(q)：重力项
拉格朗日法的推导虽然系统，但手动推导多关节机器人时，计算量非常大。我建议你用符号计算工具（比如 SymPy）来辅助：
import sympy as sp
# 定义符号变量
theta1, theta2 = sp.symbols('theta1 theta2')
dtheta1, dtheta2 = sp.symbols('dtheta1 dtheta2')
m1, m2, l1, l2, g = sp.symbols('m1 m2 l1 l2 g')
# 计算动能和势能（以两连杆为例）
# 这里省略具体表达式，实际项目中用符号推导
T = 0.5 * m1 * l1**2 * dtheta1**2 + ...
V = m1 * g * l1 * (1 - sp.cos(theta1)) + ...
# 拉格朗日量
L = T - V
# 欧拉-拉格朗日方程
# 对每个关节求导
eq1 = sp.diff(sp.diff(L, dtheta1), 't') - sp.diff(L, theta1)
eq2 = sp.diff(sp.diff(L, dtheta2), 't') - sp.diff(L, theta2)
# 化简得到 M, C, G 矩阵
用符号推导的好处是，你可以直接得到 M、C、G 的解析表达式，然后转成 C++ 或 Python 代码，效率很高。
4.5 本章小结
这一章的内容，信息量确实不小。咱们来捋一捋：
- 正运动学：从关节到末端，用 DH 参数和齐次变换矩阵搞定
- 逆运动学：从末端到关节，解析法快但受限，数值法通用但要注意奇异点
- 雅可比矩阵：关节空间和操作空间之间的速度/力映射，调试时用数值微分验证
- 动力学建模：拉格朗日法基于能量，推导系统化，建议用符号计算工具辅助
这些知识，是后续做轨迹规划、力控制、阻抗控制的基础。你想想看，没有运动学，你连机器人该往哪儿走都不知道；没有动力学，你连该用多大的力都算不出来。
嗯，这一章就到这儿。下一章咱们会把这些知识串起来，讲轨迹规划——让机器人真正「动起来」。