4. 感知坐标系与数据表示

无人飞行器要完成避障、定位、建图与目标理解，第一步不是“看见”，而是把不同传感器输出的数据放到同一个可计算的参照系里。若坐标系不统一，激光雷达测到的障碍点可能被投影到图像错误位置，视觉深度也可能与IMU积分轨迹错位。因此，本节先明确坐标系定义与变换链，再讨论点云、图像、深度图、栅格与体素五类数据表达及其适用场景。

一、三类坐标系与变换链

工程中通常约定：世界坐标系（记为 W）固定于起点或局部地图原点，Z 轴向上、X 轴指向初始航向；机体坐标系（B）原点在无人机重心，X 轴指向机头、Y 轴指向右侧、Z 轴向下，符合航空常用的前右下约定；传感器坐标系（S）原点在各传感器光心或测量中心，轴向由安装姿态决定。三者关系可写成：

p_W = R_WB · (R_BS · p_S + t_BS) + t_WB
其中 R 为旋转矩阵，t 为平移向量；
R_WB、t_WB 来自定位与姿态估计（如 VIO/INS）；
R_BS、t_BS 为传感器外参，由标定得到。

实际使用时，外参标定给出的是传感器到机体的旋转与平移，例如某前视相机的 t_BS = [0.12, 0.00, -0.05] m，表示相机光心位于重心前方 0.12 m、下方 0.05 m。若把点从传感器系转到世界系，顺序必须是“先外参、后位姿”，顺序颠倒会引入随姿态变化的误差。内参则负责把三维点投影到像素平面：u = fx·X/Z + cx，v = fy·Y/Z + cy，其中 fx、fy、cx、cy 为相机内参。只有外参、内参、位姿三者一致，点云与图像才能正确对齐。

二、点云、图像与深度图：几何与语义的分工

点云是三维空间中的离散点集合，每个点通常包含 (x, y, z, intensity)。激光雷达一圈扫描可产生数万点，优点是几何精度高、不受光照影响，缺点是稀疏、无序、缺少纹理。处理时常用体素下采样把 0.1 m 体素内的点合并，再用 KD-Tree 做近邻搜索，用于地面分割、聚类和障碍物检测。

图像是二维像素阵列，每个像素记录颜色或灰度，语义信息丰富，适合识别行人、车辆、交通标志。但单目图像缺少绝对尺度，需要借助已知目标尺寸或与IMU、激光雷达融合才能恢复距离。相机内参矩阵 K 把三维点投影到像素：

K = [[fx, 0, cx],
     [0, fy, cy],
     [0,  0,  1]]
示例：fx=460, fy=460, cx=320, cy=240（640×480 图像）

深度图为每个像素给出到相机的距离，常见来源是双目立体匹配、结构光或ToF。它比点云稠密，但边缘处易出现空洞和噪声。深度图与彩色图对齐后，可逐像素反投影为点云：X = (u-cx)·Z/fx，Y = (v-cy)·Z/fy，Z = depth(u,v)。这条反投影链是视觉与激光融合的基础。

三、栅格与体素：面向规划的空间离散

栅格地图把空间切成二维方格，每格用 0~1 的占据概率或代价值表示。例如 0.05 m 分辨率的局部栅格，无人机前方 10 m×10 m 区域对应 200×200 个格子。栅格适合二维路径搜索（A*、D*），计算量小、更新快，但无法表达悬空障碍和桥洞。

体素地图是栅格的三维推广，常用 0.1~0.3 m 体素。每个体素记录占据、空闲或未知，配合八叉树可压缩存储。体素适合三维避障、无人机穿越复杂环境时的可通行空间分析，代价是内存和计算量随分辨率立方增长。实际系统中常采用“局部体素 + 全局栅格”的分层结构：近处用体素保证三维安全，远处用栅格保证规划效率。

四、选型与使用建议

选择数据表达时，先问三个问题：任务需要几何还是语义？需要二维还是三维？对实时性和内存的约束是什么？若做语义识别，优先图像；若做精确测距与几何避障，优先点云或深度图；若做路径搜索，优先栅格；若做三维空间通行判断，优先体素。多传感器系统中，通常同时维护多种表达，并通过外参和位姿把它们统一到世界坐标系下，形成一致的环境描述。