数学基础：坐标系与变换（世界系、机体系、地图系）、向量与矩阵运算、几何基础（点、线、多边形、障碍物表示）
航迹规划的本质，是在一个被数学规则严格定义的抽象空间中寻找可行路径。无论是无人机穿越山谷、无人车在城区道路行驶，还是机械臂在工位间避障，所有规划算法最终都要落实到“在哪个坐标系下描述位置”“如何在不同坐标系之间转换”“如何用数学语言表达障碍物与可行域”这三个基本问题上。本章作为整个课程的地基，将系统梳理坐标系定义、刚体变换、向量矩阵运算以及几何图元表示方法，并重点说明这些数学工具在航迹规划中的具体应用场景与常见陷阱。
核心原理：三种坐标系的定义与内在联系
航迹规划中涉及的坐标系并非随意定义，而是根据“描述对象”和“使用场景”严格区分的。世界坐标系（World Frame，W系）是全局统一的参考基准，通常固定于地球表面某一点，例如起飞点、地图原点或UTM投影坐标原点。世界系为所有规划算法提供了位置参考，航迹点、障碍物位置、目标点均在此坐标系下定义。机体坐标系（Body Frame，B系）则固定于运动载体本身，原点通常位于载体质心或IMU中心，三个轴分别指向机头方向（X_B）、右翼方向（Y_B）和机体下方（Z_B）。机体系是传感器测量数据的天然参考系——加速度计、陀螺仪、GPS天线输出的原始数据均基于机体系。地图坐标系（Map Frame，M系）则与具体的地图表示方式绑定，例如栅格地图的像素行列、八叉树地图的体素索引、或UTM投影的东向/北向坐标。
三者之间的变换关系是航迹规划的第一道数学关卡。从机体系到世界系的变换通常由姿态估计模块（如EKF或互补滤波）输出，包含旋转矩阵R_WB和平移向量t_WB。从世界系到地图系的变换则取决于地图投影方式，例如在UTM投影中，需要将经纬度转换为平面坐标，再叠加高程信息。实际工程中，最常见的错误是混淆“机体系下的传感器读数”与“世界系下的规划坐标”。例如，激光雷达检测到的障碍物坐标是相对于雷达安装位置的，必须依次经过“雷达系→机体系→世界系”的链式变换，才能用于全局路径规划。
判断方法或操作流程：坐标变换的标准流程与矩阵运算要点
在航迹规划中，坐标变换的标准流程可以归纳为以下四个步骤：
- 明确源坐标系与目标坐标系：首先确认当前数据所在的坐标系（如机体系）以及需要转换到的目标坐标系（如世界系）。这一步看似简单，却是工程中错误率最高的环节。
- 获取变换参数：从姿态估计模块获取旋转矩阵R（或四元数q），从定位模块获取平移向量t。注意旋转矩阵必须满足正交性（R^T R = I）和行列式为+1的约束。
- 执行齐次变换：将三维点p_B扩展为齐次坐标 [p_B; 1]，左乘4×4齐次变换矩阵T_WB = [R_WB, t_WB; 0, 1]，得到世界系坐标 p_W = T_WB · p_B。
- 验证变换结果：使用已知参考点（如起飞点、标定点）进行回代验证，确保变换无漂移、无尺度错误。
向量与矩阵运算是坐标变换的底层工具。旋转矩阵R属于特殊正交群SO(3)，其逆矩阵等于转置矩阵（R⁻¹ = Rᵀ），这一性质使得反向变换（世界系→机体系）无需重新计算矩阵求逆。四元数相比欧拉角具有无万向锁、插值平滑等优势，在航迹规划的姿态插值中广泛使用。矩阵运算中需要特别注意：矩阵乘法不满足交换律（R₁R₂ ≠ R₂R₁），因此变换顺序必须严格遵循“从右到左”的链式法则。例如，将机体系下的点转换到世界系，若先经过中间坐标系I，则变换矩阵为 T_WB = T_WI · T_IB，而非 T_IB · T_WI。
| 变换类型 | 数学表达 | 典型应用场景 | 注意事项 |
|---|---|---|---|
| 机体系→世界系 | p_W = R_WB · p_B + t_WB | 将IMU/雷达数据用于全局地图构建 | R_WB需实时更新，注意姿态漂移 |
| 世界系→地图系 | p_M = f_proj(p_W) | 将GPS坐标映射到栅格地图 | 投影变形在高纬度地区显著 |
| 地图系→机体系 | p_B = R_WBᵀ · (p_W - t_WB) | 将规划航迹点转换到机体控制指令 | 注意旋转矩阵转置与平移向量顺序 |
| 多坐标系链式变换 | T_WB = T_WI · T_IB | 相机-IMU-机体-世界多级标定 | 矩阵乘法顺序不可颠倒 |
案例拆解：无人机激光雷达避障中的坐标系变换实战
以一款四旋翼无人机搭载单线激光雷达进行室内避障为例，说明坐标系变换的完整工程链路。无人机机体坐标系原点位于IMU中心，激光雷达安装在机体前方0.15m、下方0.05m处，雷达自身坐标系原点位于其旋转中心。初始现象：无人机在飞行中检测到前方障碍物，但规划模块给出的避障方向错误，导致无人机偏向障碍物飞行。
关键输入数据：激光雷达返回的障碍物极坐标（距离ρ=2.3m，角度θ=15°），IMU输出的姿态角（横滚角φ=3°，俯仰角θ=2°，偏航角ψ=30°），GPS输出的世界系位置（x=100m, y=50m, z=20m）。分析判断过程：首先将雷达极坐标转换为雷达直角坐标（x_radar = ρ·cosθ = 2.22m, y_radar = ρ·sinθ = 0.60m）。然后应用雷达安装偏移量，得到机体系坐标（x_body = 2.22 + 0.15 = 2.37m, y_body = 0.60m, z_body = -0.05m）。接着使用IMU姿态角构建旋转矩阵R_WB，将机体系坐标转换到世界系。实际操作中发现，工程师在构建旋转矩阵时错误地将俯仰角的正负号取反，导致转换后的障碍物位置偏离真实位置约0.8m，恰好使规划算法误判障碍物在安全通道内。
结果对比：修正旋转矩阵后，障碍物世界系坐标从（102.05, 51.20, 19.85）修正为（101.85, 51.80, 19.90），规划算法正确识别出该位置与预设航迹的冲突，生成了绕行路径。风险与代价：该错误若未被发现，无人机将以2m/s速度接近障碍物，在0.5s内即可发生碰撞。复盘经验：所有坐标变换代码必须包含单元测试，使用已知的标定板位置进行回代验证；同时建议在变换链路的每个环节输出中间结果，便于快速定位错误来源。
几何基础：点、线、多边形与障碍物表示方法
航迹规划中的几何表示决定了算法的复杂度与安全性。点是构成一切几何图元的基本元素，在三维空间中用向量 (x, y, z) 表示，在二维栅格地图中用行列索引 (row, col) 表示。线包括直线、射线和线段，在航迹规划中常用于表示路径段、视线检查（Line of Sight）以及Voronoi图的边。多边形是障碍物最常用的表示形式，分为凸多边形和凹多边形。凸多边形具有“任意两点连线仍在多边形内”的性质，这使得碰撞检测可以简化为“点是否在凸多边形内”的快速判断；凹多边形则需要先分解为多个凸多边形或使用更复杂的算法。
障碍物的几何表示直接影响规划效率与安全性。常见的表示方法包括：
- AABB（轴对齐包围盒）：用最小/最大坐标值定义，碰撞检测速度最快，但包围不紧密，适合粗略碰撞检测。
- OBB（有向包围盒）：可随物体旋转，包围更紧密，但碰撞检测计算量稍大。
- 凸包：用点集的最小凸包表示障碍物，适合不规则形状，但构建凸包需要O(n log n)时间。
- 栅格地图：将连续空间离散化为网格，每个格子标记为占用/空闲，是A*、Dijkstra等算法的基础。
- 八叉树：递归分割空间，在稀疏环境中节省内存，适合三维地图表示。
在工程实践中，选择障碍物表示方法需要权衡精度、内存和计算速度。例如，在高速飞行场景中，使用AABB进行快速预筛选，再对候选障碍物使用精确的凸多边形碰撞检测，可以兼顾实时性与安全性。
常见误区与总结复盘
在航迹规划数学基础的学习与工程应用中，以下误区最为常见：
- 误区一：混淆旋转矩阵与变换矩阵。旋转矩阵仅描述姿态，不包含平移；变换矩阵是4×4齐次矩阵，同时包含旋转和平移。在代码中混用两者会导致严重的定位错误。
- 误区二：忽视矩阵乘法的顺序。将“先旋转后平移”与“先平移后旋转”混为一谈。正确的齐次变换顺序是 p' = R·p + t，等价于 T·[p;1]。
- 误区三：使用欧拉角进行插值。欧拉角插值会导致姿态路径不平滑，甚至出现万向锁。航迹规划中应使用四元数球面线性插值（Slerp）或旋转矩阵的测地线插值。
- 误区四：障碍物表示过于简化。将不规则障碍物简单用圆形或AABB表示，可能导致规划路径过于保守或产生碰撞风险。应根据障碍物形状选择合适的表示方法。
- 误区五：忽略数值稳定性。长时间运行的航迹规划系统，坐标变换矩阵可能因浮点误差累积而失去正交性。应定期对旋转矩阵进行正交化修正（如使用SVD分解）。