2. 定位导航基础

在GNSS、LiDAR与IMU融合定位工程中，所有传感器数据最终都必须落到同一个空间基准、同一时间基准和同一运动描述框架中。若坐标系定义不一致、时间戳未对齐、姿态表达混用，即使单传感器精度很高，融合结果也会出现系统性漂移。本节从工程实现角度梳理五个基础支柱：坐标系、时间系统、姿态表示、运动学模型与误差基本概念。

一、坐标系：融合定位的空间基准

GNSS输出通常基于ECEF（地心地固坐标系）或WGS-84椭球下的经纬高；LiDAR点云在雷达自身坐标系下生成；IMU测量的是载体坐标系下的角速度和比力。融合前必须通过外参标定建立各传感器坐标系到载体坐标系的刚体变换，再通过载体位姿将观测统一到局部导航坐标系（如ENU）或全局ECEF中。

• ECEF：原点在地球质心，X轴指向本初子午线与赤道交点，Z轴指向北极，适合全球统一表达。
• ENU：以载体所在点为原点，东、北、天为轴，适合局部导航与可视化。
• 载体坐标系：前-右-下或前-左-上，取决于IMU安装方向，必须与车辆/机器人本体定义一致。
• 传感器坐标系：LiDAR、相机、GNSS天线各自独立，需通过外参矩阵 \(T_{s}^{b}\) 变换到载体。

二、时间系统：融合定位的时间基准

GNSS使用GPS时（GPST），IMU使用本地晶振时间，LiDAR使用内部扫描时间，操作系统可能使用UTC或单调时钟。融合定位对时间同步极其敏感：IMU积分误差随时间平方增长，LiDAR运动畸变校正依赖精确的扫描时刻。常见时间系统包括UTC、GPST、TAI与本地时间，它们之间存在跳秒和固定偏移。

GPST = UTC + 跳秒数（当前为18秒）
TAI = GPST + 19秒
本地时间 = 硬件时钟，需通过PPS或PTP与GPST对齐

工程上通常采用PPS秒脉冲加GPRMC/NMEA报文完成GNSS与本地时钟的粗同步，再用PTP或硬件触发实现微秒级对齐。所有传感器数据进入融合模块前，必须统一转换为同一时间轴，并记录时间戳来源与不确定度。

三、姿态表示：旋转的三种语言

姿态描述载体相对于导航坐标系的旋转关系，常用欧拉角、旋转矩阵、四元数三种方式。欧拉角直观但存在万向节死锁；旋转矩阵无奇异但冗余且需正交化；四元数紧凑、无奇异，适合IMU姿态递推与优化。

在融合定位中，状态量通常采用四元数或旋转矩阵的扰动形式，避免欧拉角在俯仰接近±90°时的数值不稳定。姿态更新需注意旋转顺序：常见为Z-Y-X（偏航-俯仰-横滚），不同顺序对应不同旋转矩阵。

四、运动学模型：从IMU到位姿递推

IMU提供角速度 \(\omega\) 与比力 \(a\)，在载体坐标系下测量。运动学模型描述姿态、速度、位置随时间的演化：

姿态更新：q_{k+1} = q_k ⊗ exp(0.5 * ω * Δt)
速度更新：v_{k+1} = v_k + (R(q_k) * a + g) * Δt
位置更新：p_{k+1} = p_k + v_k * Δt + 0.5 * (R(q_k) * a + g) * Δt²

其中 \(R(q)\) 为四元数对应的旋转矩阵，\(g\) 为重力向量。该递推是IMU预积分的核心，也是GNSS/LiDAR观测与IMU预测之间建立残差的基础。工程中需注意：加速度计测量的是比力而非绝对加速度，必须扣除重力；角速度积分需采用中值或圆锥补偿以降低高频误差。

五、误差基本概念：融合定位的敌人

融合定位中的误差可分为确定性误差与随机误差。确定性误差包括零偏、尺度因子、安装误差、杆臂；随机误差包括高斯白噪声、随机游走、量化噪声。IMU零偏随时间缓慢变化，是航位推算漂移的主因；GNSS多路径与LiDAR点云配准误差则表现为观测异常。

• 零偏：角速度/加速度输出的常值偏移，需在线估计或标定补偿。
• 尺度因子：输出与真实值的比例误差，影响长距离积分。
• 杆臂：IMU与GNSS天线/LiDAR之间的平移，动态下产生附加速度与位置误差。
• 时间偏移：传感器时间戳不同步，等效为动态误差。
• 随机游走：零偏随时间的随机变化，需用Allan方差分析建模。

误差建模的目标是让滤波器或优化器知道“该信谁、信多少”。协方差矩阵、观测权重与鲁棒核函数都是误差建模的工程手段。

六、案例与复盘：一次融合漂移的排查

某园区巡检机器人融合定位出现“直行时航向缓慢偏转、转弯后位置跳变”。排查发现：IMU安装方向与载体定义相差90°，外参旋转矩阵未正确写入；同时LiDAR时间戳使用本地时钟，与GPST存在约120ms固定偏移。修正外参并启用PPS对齐后，航向漂移从每分钟3°降至0.2°以内，转弯跳变消失。

工程落地建议：建立统一的标定与时间同步检查清单，每次上车前验证外参、时间偏移与IMU零偏；在融合框架中显式维护状态量、协方差与观测残差，使误差可观测、可追溯、可补偿。