第二章 机器人运动学基础：刚体运动描述、齐次变换矩阵、正运动学与逆运动学入门（以2轮差速模型为例）
各位同学，欢迎来到第二章。
这一章，我们聊聊机器人运动学。说实话，这是整个视觉引导和运动规划的基石。你想想看，如果连机器人怎么动、动到哪里去都说不清楚，那后面的避障、路径规划根本无从谈起。
我个人习惯把运动学比作「机器人的语言」。你得先学会这门语言，才能跟机器人沟通。今天我们就从最基础的刚体运动描述开始，一步步深入到齐次变换矩阵，最后用2轮差速模型把正逆运动学讲明白。
2.1 刚体运动描述：位置与姿态
机器人本质上是一个刚体。刚体在空间中的运动，说白了就是两件事：它在哪里（位置），以及它朝哪个方向（姿态）。
位置很好理解，就是三维空间里的一个点，用坐标 (x, y, z) 表示。但姿态就有点意思了。我记得刚入行时，总以为姿态就是「转了个角度」，后来发现完全不是那么回事。
描述姿态，常用的有三种方式：
- 旋转矩阵：3x3的矩阵，最直观，但参数冗余（9个参数描述3个自由度）。
- 欧拉角：绕固定轴依次旋转，比如 Roll-Pitch-Yaw。简单易懂，但有万向锁问题。
- 四元数：4个参数，无奇异性，插值平滑。我在做无人机项目时，姿态估计全用的四元数。
这里我给大家一个建议：做视觉引导时，尽量用四元数。为什么？因为视觉SLAM输出的姿态基本都是四元数，你硬要转成欧拉角，反而容易引入麻烦。
2.2 齐次变换矩阵：把位置和姿态打包
有了位置和姿态，我们怎么把它们放在一起？答案就是齐次变换矩阵。
齐次变换矩阵是一个 4x4 的矩阵，长这样：
| R t |
| 0 1 |其中 R 是 3x3 的旋转矩阵，t 是 3x1 的平移向量。最后一行是 [0 0 0 1]。
为什么要加最后一行？说白了，就是为了把旋转和平移统一成矩阵乘法。你想想看，如果没有齐次坐标，你要先旋转再平移，得写两个式子。有了齐次变换矩阵，一个矩阵乘法就搞定了。
我曾经在做一个机械臂抓取项目时，需要把相机坐标系下的物体位姿转换到机器人基坐标系下。当时就是靠齐次变换矩阵链式相乘，一行代码搞定坐标变换。嗯，那种感觉真的很爽。
2.3 2轮差速模型：正运动学
好了，理论讲完了，我们来看一个具体的例子——2轮差速模型。这是移动机器人最经典的底盘结构，像扫地机器人、AGV小车，基本都是这个模型。
2轮差速模型的核心思想：两个轮子独立驱动，通过转速差实现转向。
先定义几个变量：
- v：机器人线速度（m/s）
- ω：机器人角速度（rad/s）
- v_L：左轮线速度
- v_R：右轮线速度
- d：轮距（两个轮子中心的距离）
- r：轮子半径
正运动学，就是已知左右轮速度，求机器人整体的速度。公式很简单：
v = (v_R + v_L) / 2
ω = (v_R - v_L) / d你看，线速度是两个轮子速度的平均值，角速度是速度差除以轮距。这个公式我闭着眼睛都能写出来，因为调试过太多次了。
有了 v 和 ω，我们就可以计算机器人在世界坐标系下的位置变化：
dx/dt = v * cos(θ)
dy/dt = v * sin(θ)
dθ/dt = ω这就是经典的航迹推算公式。我在做AGV导航时，就是靠这个公式，配合编码器数据，实时估计机器人位姿。当然，编码器有累积误差，时间长了会漂移，所以需要视觉或激光雷达做修正。
2.4 2轮差速模型：逆运动学
逆运动学，就是已知目标速度 (v, ω)，求左右轮应该转多快。
公式同样简单：
v_R = v + (ω * d) / 2
v_L = v - (ω * d) / 2然后根据轮子半径 r，换算成电机转速：
ω_R = v_R / r
ω_L = v_L / r这里有个坑，我曾经踩过。如果你直接给电机发转速指令，会发现机器人走不直。为什么？因为左右电机的特性不可能完全一致，哪怕你给同样的转速，实际速度也有微小差异。
我的解决办法是：加一个闭环控制。用编码器实时读取实际转速，然后通过PID调节，让左右轮的实际速度跟上目标值。这样机器人才能走直线。
2.5 知识体系总览
为了让大家更直观地理解本章的知识结构，我画了一张图：
这张图把本章的知识脉络梳理得很清楚。从刚体运动描述出发，到齐次变换矩阵，再到2轮差速模型的正逆运动学，环环相扣。
2.6 本章小结
这一章我们讲了三个核心内容：
- 刚体运动描述：位置用坐标，姿态用旋转矩阵/欧拉角/四元数。我推荐用四元数。
- 齐次变换矩阵：把位置和姿态打包成4x4矩阵，方便做坐标变换。逆变换就是求逆矩阵。
- 2轮差速模型：正运动学从轮速求机器人速度，逆运动学从目标速度求轮速。别忘了加闭环控制。
说实话，这些内容看起来简单，但真正用好并不容易。我在实际项目中见过太多人把正逆运动学搞反，或者忘记考虑轮距。嗯，多练练就好了。