第1章：基础数学回顾——向量与坐标系、刚体运动与姿态表示
各位同学，欢迎来到《速度约束下的路径跟踪》课程。我是你们的老朋友，一个在机器人控制领域摸爬滚打多年的工程师。
今天咱们聊点基础，但别小看基础。我见过太多项目，最后出问题都出在最基础的数学概念上。说白了，向量、坐标系、姿态表示这些玩意儿，就是咱们和机器人沟通的“语言”。语言不通，活儿肯定干不好。
1.1 向量与坐标系：机器人的“世界观”
机器人怎么知道自己在哪？它得有个参考系。这就是坐标系。
世界坐标系，就是咱们定义的“绝对”位置。比如房间的东北角是原点，X轴朝东，Y轴朝北。机器人说“我在(1,2)”，就是相对于这个原点说的。
机器人坐标系，是绑在机器人身上的。比如车头方向是X轴，左侧是Y轴。传感器测得“前方1米有障碍”，这个“前方”就是机器人自己的坐标系。
这两个坐标系之间怎么转换？靠向量。
核心概念：一个向量，在不同坐标系下有不同的坐标值。但向量本身（长度、方向）是不变的。
举个例子。我在做AGV（自动导引车）项目时，激光雷达装在车头，它测到障碍物的坐标是相对于雷达的。但我要控制的是车体中心，所以必须把雷达坐标转换到车体坐标系，再转换到世界坐标系。一步错，步步错。
坐标变换公式很简单：
// 二维情况
// 已知：点P在坐标系A中的坐标 (x_A, y_A)
// 坐标系A到坐标系B的平移向量 t_AB
// 旋转矩阵 R_AB
// 求：点P在坐标系B中的坐标 (x_B, y_B)
x_B = R_AB[0][0] * x_A + R_AB[0][1] * y_A + t_AB[0]
y_B = R_AB[1][0] * x_A + R_AB[1][1] * y_A + t_AB[1]嗯，这里要注意：旋转矩阵是正交矩阵，它的逆等于它的转置。这个性质在后续推导中会反复用到。
1.2 刚体运动与姿态表示：让机器人“动起来”
刚体运动，说白了就是“平移+旋转”。机器人不会变形，所以它的运动就是这两个动作的组合。
姿态表示，是描述机器人“朝哪看”的数学工具。常用的有三种：
- 旋转矩阵：9个元素，冗余大，但直观，适合推导公式。
- 欧拉角：3个角度（横滚、俯仰、偏航），直观但容易遇到“万向锁”。
- 四元数：4个元素，无奇点，适合插值和计算。
我个人习惯，在控制算法里用四元数，在可视化或人机交互时转成欧拉角。为什么？因为四元数做插值平滑，不会像欧拉角那样突然“卡住”。
避坑指南：我曾经在无人机项目中，直接用欧拉角做姿态控制。结果飞机做大角度机动时，横滚和偏航突然耦合，差点炸机。后来换成四元数，问题就解决了。记住：大角度运动，别用欧拉角。
旋转矩阵
旋转矩阵R是一个3x3的正交矩阵，满足R^T R = I，det(R)=1。它把向量从一个坐标系旋转到另一个坐标系。
比如绕Z轴旋转θ角：
R_z(θ) = [
[cosθ, -sinθ, 0],
[sinθ, cosθ, 0],
[0, 0, 1]
]连续旋转就是矩阵乘法。注意顺序：先绕X轴转，再绕Y轴转，对应的旋转矩阵是 R = R_y * R_x。顺序不能乱，因为矩阵乘法不满足交换律。
四元数
四元数 q = (w, x, y, z)，其中w是实部，(x,y,z)是虚部。它表示绕单位向量 (x,y,z) 旋转 2*arccos(w) 角度。
为什么用四元数？因为它没有奇点，而且插值简单。比如两个姿态之间做平滑过渡，用四元数的球面线性插值（SLERP）就行。
// 四元数乘法（用于组合旋转）
q1 * q2 = (w1*w2 - x1*x2 - y1*y2 - z1*z2,
w1*x2 + x1*w2 + y1*z2 - z1*y2,
w1*y2 - x1*z2 + y1*w2 + z1*x2,
w1*z2 + x1*y2 - y1*x2 + z1*w2)
// 用四元数旋转向量 v
v_rot = q * v * q_conj // q_conj 是 q 的共轭注意：四元数虽然好，但调试时很难直观看出“朝哪”。我一般会在代码里同时打印欧拉角，方便肉眼检查。但控制回路里只用四元数，避免奇点。
1.3 运动学基础：机器人怎么“走”
运动学，研究的是位置、速度、加速度之间的关系，不考虑力。说白了，就是“给定车轮转速，机器人会怎么动”。
对于轮式机器人，最经典的是差速驱动模型：
- 左右轮速度分别为 v_l, v_r
- 轮距为 L
- 机器人线速度 v = (v_l + v_r) / 2
- 机器人角速度 ω = (v_r - v_l) / L
你想想看，如果左右轮速度一样，机器人直行。如果左轮慢右轮快，机器人右转。就这么简单。
但实际项目中，轮子会打滑，电机有响应延迟。所以运动学模型只是“理想情况”，真正控制时还得加反馈。
我记得有一次，在工厂里调试AGV，运动学模型算出来路径很完美，但实际跑起来总是偏。后来发现是地面有油渍，轮子打滑了。从那以后，我每次都会在模型里加一个“打滑因子”，虽然不精确，但至少能兜底。
1.4 知识体系总览
下面这张图，是我自己画的本章知识结构。你可以把它当作一个“地图”，后续章节都会围绕这些基础展开。
这张图里，三个分支是并列关系。但实际应用中，它们是交织在一起的。比如你要做路径跟踪，得先知道机器人当前在世界坐标系下的位置（坐标系变换），然后知道它朝哪看（姿态表示），最后才能算出怎么控制轮子（运动学模型）。
好了，第一章就到这里。这些基础概念，后续章节会反复用到。如果你现在觉得有点模糊，没关系，后面我会结合具体案例再讲。
可可精品资料库