第2章：数学基础回顾——向量、矩阵、坐标系与刚体运动

各位好，欢迎来到《轨迹规划实战指南》的第二章。

说实话，每次开始讲轨迹规划，我都要先花点时间把数学基础捋一遍。为什么？因为轨迹规划本质上就是一连串的数学运算——你在三维空间里让机器人从A点走到B点，背后全是向量加减、矩阵乘法、坐标系变换这些东西。

我见过不少同学，算法思路很清晰，但一写到代码就卡壳。问题往往出在数学基础上——旋转矩阵用错了、齐次坐标忘了加、四元数搞反了顺序。嗯，今天我们就把这些坑一个一个填上。

2.1 向量与矩阵运算

先聊向量。在轨迹规划里，向量就是空间中的一个点或者一个方向。比如机器人末端的位置 p = [x, y, z]^T，或者它要移动的方向 v = [vx, vy, vz]^T。

我个人习惯把向量看作列向量，这样跟矩阵乘法配合起来更自然。你想想看，一个3x3矩阵乘以一个3x1列向量，结果还是3x1列向量，多整齐。

2.1.1 基本运算

• 点积：a · b = a^T b = a₁b₁ + a₂b₂ + a₃b₃。用来算夹角、投影长度。我在做路径平滑时经常用点积判断两个方向是否一致。
• 叉积：a × b，结果是一个垂直于a和b的向量。用来算法向量、力矩。注意叉积不满足交换律，a × b = -b × a。
• 模长：||a|| = sqrt(a·a)。归一化时必用。

2.1.2 矩阵乘法与逆矩阵

矩阵乘法说白了就是线性变换的复合。旋转矩阵乘以向量，得到旋转后的向量；两个旋转矩阵相乘，得到复合旋转。

这里有个坑——矩阵乘法不满足交换律。AB和BA通常不一样。我在项目中遇到过好几次，调试半天发现是矩阵乘反了。所以写代码时一定要看清楚是左乘还是右乘。

运算	符号	维度要求	注意事项
矩阵乘法	C = A B	A: m×n, B: n×p → C: m×p	不交换，左乘右乘区别大
转置	A^T	m×n → n×m	旋转矩阵的逆等于转置
逆矩阵	A^{-1}	方阵，行列式≠0	数值不稳定时用伪逆

2.2 坐标系与变换（齐次坐标）

坐标系变换是轨迹规划里最绕的部分。你有一个世界坐标系，机器人身上有基坐标系、工具坐标系、相机坐标系……每个传感器都有自己的坐标系。怎么把这些坐标统一起来？

答案就是——齐次坐标。

2.2.1 为什么需要齐次坐标？

普通的3D变换，旋转用3x3矩阵，平移用3x1向量。但如果你想把旋转和平移写成一个运算，就得用4x4齐次变换矩阵。

齐次坐标把一个3D点 [x, y, z]^T 扩展成 [x, y, z, 1]^T。这样，一个4x4矩阵就能同时表示旋转和平移：

T = [ R   t ]
    [ 0^T 1 ]

其中 R 是 3x3 旋转矩阵，t 是 3x1 平移向量。

变换一个点：p' = T p，一步到位。

2.2.2 链式变换

在实际系统中，坐标变换往往是链式的。比如从世界坐标系到相机坐标系，再到物体坐标系：

T_world_to_cam = T_world_to_base * T_base_to_cam
T_cam_to_obj   = ...

我习惯用树状结构来管理这些变换关系。每个坐标系是一个节点，变换矩阵是边。这样不管有多少坐标系，都能通过遍历树来找到任意两个坐标系之间的变换。

2.3 刚体运动描述

刚体运动有6个自由度——3个平移、3个旋转。平移好说，就是向量加法。旋转就复杂了，有旋转矩阵、欧拉角、轴角、四元数等多种表示方法。

我个人最常用的是旋转矩阵和四元数。欧拉角虽然直观，但有万向锁问题，轨迹规划里尽量少用。

2.3.1 旋转矩阵

旋转矩阵是一个3x3正交矩阵，行列式为+1。它的每一列都是单位向量，且两两正交。

绕X轴旋转θ角：

R_x(θ) = [1      0       0   ]
         [0   cosθ   -sinθ  ]
         [0   sinθ    cosθ  ]

绕Y轴旋转θ角：

R_y(θ) = [ cosθ   0   sinθ ]
         [ 0      1   0    ]
         [-sinθ   0   cosθ ]

绕Z轴旋转θ角：

R_z(θ) = [cosθ  -sinθ  0 ]
         [sinθ   cosθ  0 ]
         [0      0     1 ]

复合旋转就是矩阵乘法。注意顺序——先绕X再绕Y，和先绕Y再绕X，结果完全不同。

2.3.2 四元数

四元数是个好东西。它没有万向锁，插值平滑，计算效率高。在轨迹规划里，做姿态插值时几乎必用四元数。

四元数 q = w + xi + yj + zk，其中 w 是实部，(x, y, z) 是虚部。单位四元数表示旋转：

q = [cos(θ/2), sin(θ/2) * u]

其中 u 是旋转轴单位向量，θ 是旋转角度。

四元数乘法：

q1 * q2 = [w1*w2 - v1·v2, w1*v2 + w2*v1 + v1×v2]

用四元数旋转一个向量 v：

v' = q * [0, v] * q^{-1}

其中 q^{-1} = [w, -x, -y, -z]（单位四元数）。

2.3.3 旋转矩阵与四元数的互转

转换方向	公式	注意事项
四元数 → 旋转矩阵	R = 2*[w²+x²-0.5, xy-wz, xz+wy; xy+wz, w²+y²-0.5, yz-wx; xz-wy, yz+wx, w²+z²-0.5]	确保四元数已归一化
旋转矩阵 → 四元数	w = 0.5*sqrt(1+R₁₁+R₂₂+R₃₃)	注意数值稳定性，避免除零

2.4 知识体系总览

下面这张图把本章的核心知识点串起来了。你可以把它当作一个快速索引，以后遇到问题回来翻一翻。

2.5 实战建议

学完这些数学基础，我建议你马上做两件事：

1. 写一个齐次变换矩阵类，支持旋转、平移、复合变换、逆变换。用单元测试验证每个功能。
2. 实现四元数球面线性插值（SLERP）。这是轨迹规划里做姿态平滑的标配。代码不超过50行，但能帮你彻底理解四元数的几何意义。