RTK、IMU与激光雷达组合导航算法：原理、实现与验证

一、组合导航系统原理与数学模型

1.1 传感器特性与互补性分析

• RTK（实时动态差分定位）：提供厘米级绝对位置精度，采样频率1-10Hz，易受遮挡影响[54]
• IMU（惯性测量单元）：输出高频（100-1000Hz）角速度与加速度，短期精度高但存在累积误差[82]
• 激光雷达：提供环境深度信息，采样频率10-20Hz，抗光照干扰能力强[184]

三者组合可实现：

• 时间互补：IMU高频数据弥补RTK/激光雷达的低频缺陷
• 空间互补：RTK提供全局绝对定位，激光雷达提供相对环境约束
• 误差互补：激光雷达与RTK共同抑制IMU的累积漂移[28]

1.2 系统状态方程构建

采用误差状态卡尔曼滤波（ESKF） 框架，状态向量定义为：

\mathbf{x} = \begin{bmatrix}
\delta\mathbf{p}^T & \delta\mathbf{v}