数学基础（一）：空间描述与变换

做机器人轨迹规划，第一件事就是搞清楚——物体在哪儿？它朝哪个方向？

你想想看，机械臂要抓一个杯子，你得知道杯子相对于机械臂底座的位置和姿态。这就是空间描述。而要让机械臂从A点运动到B点，中间还得做变换。说白了，这一章就是整个轨迹规划的坐标系语言。

我个人习惯把空间描述和变换比作「给机器人装GPS和指南针」。没有这个基础，后面所有轨迹算法都是空中楼阁。

2.1 位置与姿态表示

先说位置。三维空间里一个点，用三个坐标就够了：

P = [x, y, z]ᵀ

但光有位置不够。你让机械臂末端走到某个点，如果末端朝向不对，照样抓不住东西。所以还需要姿态——描述这个物体是怎么「摆」的。

姿态的数学表示有好几种：旋转矩阵、欧拉角、四元数。每种都有各自的脾气。我在项目中遇到过因为欧拉角万向锁导致机械臂「抽风」的情况，后面细说。

2.2 旋转矩阵

旋转矩阵是3×3的正交矩阵，行列式为+1。它把一个坐标系中的向量映射到另一个坐标系。

R = [r₁₁ r₁₂ r₁₃; 
     r₂₁ r₂₂ r₂₃; 
     r₃₁ r₃₂ r₃₃]

性质：RᵀR = I, det(R) = +1

绕三个坐标轴的旋转矩阵是基础：

旋转轴	旋转矩阵（角度θ）
X轴	[1 0 0; 0 cosθ -sinθ; 0 sinθ cosθ]
Y轴	[cosθ 0 sinθ; 0 1 0; -sinθ 0 cosθ]
Z轴	[cosθ -sinθ 0; sinθ cosθ 0; 0 0 1]

旋转矩阵的好处是直观、无奇异。坏处是9个参数只有3个自由度，冗余大，做插值不方便。

2.3 欧拉角

欧拉角用三个角度描述姿态。常见的有ZYX顺序（偏航-俯仰-滚转）：

R = Rz(ψ) · Ry(θ) · Rx(φ)

ψ：偏航角（绕Z轴）
θ：俯仰角（绕Y轴）
φ：滚转角（绕X轴）

欧拉角很直观，人脑容易理解。但有个大坑——万向锁。

万向锁的本质：当第二个旋转角为±90°时，第一个和第三个旋转轴重合，丢失一个自由度。这不是算法bug，是数学上的奇点。

2.4 四元数

四元数是解决姿态表示问题的终极方案。它没有奇点，插值平滑，计算效率高。

q = w + xi + yj + zk

其中 w² + x² + y² + z² = 1（单位四元数）

四元数表示绕单位向量 u 旋转角度 θ：

q = [cos(θ/2), ux·sin(θ/2), uy·sin(θ/2), uz·sin(θ/2)]

四元数乘法对应旋转复合：

q₁ ⊗ q₂ = [w₁w₂ - v₁·v₂, w₁v₂ + w₂v₁ + v₁×v₂]

表示方法	参数数量	奇点	插值	计算效率
旋转矩阵	9	无	困难	中等
欧拉角	3	有	一般	高
四元数	4	无	平滑	高

2.5 齐次变换矩阵

位置和姿态分开处理太麻烦。齐次变换矩阵把它们合在一起：

T = [R   p;
     0ᵀ  1]

其中 R 是3×3旋转矩阵，p 是3×1位置向量

齐次变换的好处：

• 统一表示：一个矩阵搞定旋转+平移
• 链式复合：T₁₂ · T₂₃ = T₁₃，直接连乘
• 求逆方便：T⁻¹ = [Rᵀ -Rᵀp; 0 1]

举个例子，机械臂从基座到末端：

T_base_to_end = T_base_to_j1 · T_j1_to_j2 · T_j2_to_j3 · ... · T_jn_to_end

这就是正运动学的核心。每个关节的变换矩阵连乘，得到末端位姿。

2.6 知识体系总览

下面这张图把本章的核心概念串起来：

2.7 实用建议与避坑指南

做轨迹规划这些年，我总结了几条铁律：

• 统一坐标系：整个系统只用一套约定（比如右手系、Z轴向上），别混用。我见过因为坐标系不统一导致机器人撞墙的事故。
• 四元数归一化：每次运算后检查 ||q||=1，浮点误差会慢慢漂移。不归一化的话，旋转会变形。
• 欧拉角只用于显示：内部计算用四元数或旋转矩阵，只在最后显示给用户时转成欧拉角。
• 齐次变换矩阵的逆：别直接求逆矩阵，用公式 [Rᵀ -Rᵀp; 0 1] 算，又快又准。

嗯，这一章的内容就到这里。空间描述与变换是轨迹规划的地基，地基不稳，上面盖什么都白搭。把这些概念吃透，后面学运动学、动力学、轨迹生成才会顺手。