4. 速度与静力学：速度雅可比矩阵、力雅可比矩阵、静力平衡与对偶关系
好，咱们进入第四章。这一章，说白了就是打通「运动」和「力」之间的任督二脉。
你想想看，机器人末端在空间里跑得飞快，那它的关节得转多快？反过来，末端要顶住一个重物，关节电机又该出多大的力？这两个问题，本质上是一回事。嗯，这就是雅可比矩阵要干的事。
4.1 速度雅可比矩阵：关节速度 → 末端速度
先看正向运动学。我们有一个函数，把关节角度 q 映射到末端位姿 x：
x = f(q)两边对时间求导，链式法则一用：
ẋ = J(q) · q̇这个 J(q) 就是速度雅可比矩阵。它是一个 6×n 的矩阵（n 是关节数），前3行对应线速度，后3行对应角速度。
核心理解：雅可比矩阵是「关节空间到任务空间」的线性映射。在任意一个构型 q 下，它告诉你关节转一点点，末端会怎么动。
我在项目中遇到过一件事：有一次调试一个6轴协作臂，末端画圆总是有畸变。查了半天，发现是雅可比矩阵在奇异位形附近病态了。嗯，这就是为什么后面要讲奇异值分解。
4.1.1 如何计算雅可比矩阵
对于旋转关节，第 i 列的计算公式是：
J_i = [ z_i × (p_n - p_i) ]
[ z_i ]其中 z_i 是第 i 个关节的旋转轴方向，p_i 是关节原点位置，p_n 是末端位置。说白了，就是每个关节对末端速度的贡献。
我的习惯：写代码时，我会先把所有关节的变换矩阵算出来，然后逐列提取 z 轴和原点位置。这样不容易出错。
4.2 力雅可比矩阵：末端力 → 关节力矩
现在反过来。末端受到一个力 F（包含力和力矩），关节上需要输出多大的力矩 τ？
答案是：
τ = J^T · F没错，就是雅可比矩阵的转置。这个关系叫「静力对偶」。你想想看，速度是正向用 J，力是反向用 J^T，多漂亮的对偶关系。
我曾经踩过的坑：力雅可比用的是转置，不是逆矩阵！只有在非奇异位形下，J 才可逆。但力映射永远用转置，这是由虚功原理决定的。
4.2.1 虚功原理推导
虚功原理说：关节做的虚功 = 末端做的虚功。
τ^T · δq = F^T · δx而 δx = J · δq，代入得：
τ^T · δq = F^T · J · δq两边消去 δq，得到：
τ = J^T · F嗯，就这么简单。但背后是能量守恒，这是机器人学的基石之一。
4.3 静力平衡与对偶关系
我们把两个关系放在一起看：
| 方向 | 映射关系 | 矩阵 |
|---|---|---|
| 关节 → 末端（速度） | ẋ = J · q̇ | J |
| 末端 → 关节（力） | τ = J^T · F | J^T |
这就是对偶关系。速度从关节流向末端，力从末端流回关节。两个方向共用同一个矩阵，只是转置一下。
实际意义：你设计力控算法时，本质上就是在 J^T 上做文章。比如阻抗控制，就是通过 J^T 把末端的虚拟弹簧力映射到关节力矩上。
4.3.1 静力平衡的条件
当机器人静止时（或者匀速运动时），关节力矩必须平衡末端外力加上重力。写成方程：
τ = J^T · F_ext + g(q)其中 g(q) 是重力补偿项。我在做搬运机器人时，发现如果不加重力补偿，末端稍微偏离水平，关节力矩就全错了。嗯，这个坑我记忆犹新。
4.4 知识体系结构图
下面这张图，把本章的核心逻辑串起来了。你看一遍，应该能记住整个框架。
4.5 代码示例：计算平面2R机器人的雅可比
光说不练假把式。咱们写个简单的平面2R机器人雅可比计算。你可以在 ROS 里直接跑这个思路。
import numpy as np
def planar_2r_jacobian(q, l1, l2):
"""
计算平面2R机器人的速度雅可比矩阵
q: [theta1, theta2] 关节角度
l1, l2: 连杆长度
返回: 2x2 雅可比矩阵 (只考虑末端位置，不考虑姿态)
"""
theta1, theta2 = q
c1 = np.cos(theta1)
s1 = np.sin(theta1)
c12 = np.cos(theta1 + theta2)
s12 = np.sin(theta1 + theta2)
# 末端位置
x = l1 * c1 + l2 * c12
y = l1 * s1 + l2 * s12
# 雅可比矩阵的列
J11 = -l1 * s1 - l2 * s12
J12 = -l2 * s12
J21 = l1 * c1 + l2 * c12
J22 = l2 * c12
J = np.array([[J11, J12],
[J21, J22]])
return J
# 示例：在某个构型下计算
q_test = np.array([0.5, 0.3])
J = planar_2r_jacobian(q_test, 1.0, 0.8)
print("雅可比矩阵：\n", J)
# 给定关节速度，求末端速度
q_dot = np.array([0.2, 0.1])
x_dot = J @ q_dot
print("末端速度：", x_dot)
# 给定末端力，求关节力矩
F = np.array([10.0, 0.0])
tau = J.T @ F
print("关节力矩：", tau)
我建议：你在 ROS 里写机器人控制器时，把雅可比计算封装成一个单独的函数。这样调试力控、速度前馈、奇异点检测都方便。我自己的库里，雅可比计算和运动学是分开的，测试起来特别清爽。
4.6 避坑指南
- 奇异位形：当雅可比矩阵不满秩时，机器人会失去某个方向的运动能力。我曾经在调试时，机器人手臂伸直了，末端死活没法沿某个方向动。嗯，那就是奇异点。
- 力控的稳定性：用 J^T 做力控时，如果雅可比矩阵病态，力矩会突然变得很大。我建议加一个阻尼项，或者做奇异值截断。
- 坐标系一致性：雅可比矩阵的每一列，必须定义在同一个坐标系下。我习惯把所有向量都转到基坐标系下计算，省得搞混。
好了，这一章就到这里。速度雅可比和力雅可比，说白了就是一对孪生兄弟。你掌握了 J 和 J^T，就等于掌握了机器人运动学和静力学的半壁江山。
可可精品资料库