第二讲：运动学基础——位置、速度、加速度的数学关系，雅可比矩阵与速度映射

各位同学，欢迎来到第二讲。

上一讲我们聊了轨迹规划的整体框架。今天要深入一个基础但极其重要的内容——运动学。说白了，就是搞清楚机器人各个关节动起来时，末端到底怎么走。

我刚开始做机器人项目时，总觉得运动学就是背公式。直到有一次，我设计的轨迹在仿真里跑得完美，一上真机就抖得像筛糠。后来才发现，是速度映射没算对。嗯，从那以后，我再也不敢小看这一讲了。

1. 位置、速度、加速度：时间维度上的三兄弟

先看最简单的单自由度系统。比如一个滑块在导轨上滑动。

位置 s(t) 是时间的函数。速度是位置对时间的一阶导数：

v(t) = ds(t) / dt

加速度是速度对时间的一阶导数，也是位置的二阶导数：

a(t) = dv(t) / dt = d²s(t) / dt²

反过来，已知加速度，积分一次得速度，再积分一次得位置。这就是运动学正解的基本思路。

核心要点：

位置连续，速度不一定连续（比如急停）
速度连续，加速度不一定连续（比如加速度突变）
轨迹规划的目标之一，就是让加速度也连续——这样机器人运动才平滑

我个人习惯在规划轨迹时，先看加速度曲线。如果加速度有跳变，那大概率会出问题。你想想看，电机突然要输出一个阶跃的力，机械结构受得了吗？

2. 从单关节到多关节：雅可比矩阵登场

单关节好办。但机器人通常有6个、7个关节。关节空间的速度，怎么映射到末端执行器的速度？

答案就是雅可比矩阵 J。

设关节角度向量为 q = [q₁, q₂, ..., qₙ]ᵀ，末端位姿向量为 x = [x, y, z, α, β, γ]ᵀ。

那么：

ẋ = J(q) · q̇

其中 ẋ 是末端速度（线速度+角速度），q̇ 是关节速度。

雅可比矩阵的每一列，就是对应关节运动对末端速度的贡献。说白了，它是一个线性映射——在某个位形下，把关节速度线性变换成末端速度。

我的经验：

雅可比矩阵是时变的。机器人换个姿势，J就变了。所以千万别把某个位形下的雅可比拿去算另一个位形的速度映射——我曾经犯过这个错，结果末端速度算出来差了30%。

3. 雅可比矩阵的几何意义

雅可比矩阵其实在描述一件事：每个关节转动一点点，末端会怎么动。

对于旋转关节，第i列雅可比可以写成：

J_i = [z_i × (p_end - p_i); z_i]

其中：

z_i 是第i关节的旋转轴方向
p_i 是第i关节的位置
p_end 是末端位置
上半部分 z_i × (p_end - p_i) 是线速度贡献
下半部分 z_i 是角速度贡献

这个公式我建议你手推一遍。我在项目中遇到过，有些同事只会用工具箱算雅可比，一旦工具箱出bug，根本不知道哪里错了。手推一遍，你就能理解每一列的物理含义。

4. 速度映射的逆问题：逆雅可比

实际控制中，我们往往知道末端要往哪走（期望的ẋ），需要反算关节速度（q̇）。

q̇ = J⁻¹(q) · ẋ

这里有个大坑：雅可比矩阵不一定可逆。

避坑指南：

当机器人处于奇异位形时，雅可比矩阵降秩，逆矩阵不存在。此时，某些方向的末端速度无法实现。

我曾经在调试一个6轴机器人时，末端走到工作空间边界附近，突然报错说速度计算发散。一查，原来是雅可比接近奇异，数值不稳定。后来我加了阻尼最小二乘法（DLS）来处理，才稳定下来。

处理奇异位形的常用方法：

q̇ = Jᵀ · (J · Jᵀ + λ²I)⁻¹ · ẋ

其中λ是阻尼系数。λ越大，越稳定，但精度会下降。这是个trade-off。

5. 加速度映射：别忘了科里奥利项

速度映射是线性的。但加速度映射不是。

对 ẋ = J(q) · q̇ 两边求导：

ẍ = J(q) · q̈ + J̇(q) · q̇

注意，多出来一项 J̇(q) · q̇。这一项就是科里奥利加速度和向心加速度的来源。

很多初学者只算 J · q̈，忽略了 J̇ · q̇。结果就是：末端加速度算出来偏小，轨迹跟踪误差越来越大。

记住：

高速运动时，科里奥利项不可忽略。我做过一个码垛项目，机器人末端速度达到2m/s，忽略科里奥利项后，轨迹误差超过5mm。加上之后，误差降到0.3mm以内。

6. 知识体系总览

下面这张图，是我自己总结的运动学基础核心逻辑。建议你保存下来，每次做轨迹规划前看一眼。

7. 实战中的几个小技巧

数值雅可比 vs 解析雅可比：数值雅可比用差分近似，简单但精度低。解析雅可比用公式推导，精度高但实现复杂。我建议在仿真阶段用数值雅可比快速验证，上真机前换成解析雅可比。
雅可比矩阵的维度检查：6自由度机器人，雅可比是6×6矩阵。如果算出来不是这个维度，赶紧检查代码。
速度映射的验证方法：给一个很小的关节增量Δq，用正运动学算末端位移Δx，然后比较 Δx/Δt 和 J·q̇。如果两者接近，说明雅可比算对了。

一个小工具：

我习惯在代码里写一个 check_jacobian() 函数，每次启动时自动做一次数值验证。花不了多少时间，但能避免很多低级错误。

好了，这一讲的内容就是这些。运动学基础是轨迹规划的基石，雅可比矩阵更是核心中的核心。建议你找一个小型机器人模型，手算一遍雅可比，再写代码验证。相信我，做完这一步，你对速度映射的理解会上一个台阶。