3. 移动平台定位
移动操作机器人要在同一套坐标体系下完成“走到哪里”和“操作什么”两件事，定位是融合的前提。移动平台定位并不是单一传感器能独立完成的任务，而是由轮式里程计、惯性测量单元（IMU）、GNSS、激光雷达和视觉定位共同构成的多源体系。不同传感器在频率、精度、漂移特性、环境依赖和成本上差异明显，理解它们的原理与优缺点，才能在设计融合方案时做出合理取舍。
一、五类定位源的基本原理
轮式里程计通过编码器测量左右轮转角，结合轮半径、轮距和运动学模型推算位姿。差速底盘常用公式为：
Δs = (Δs_L + Δs_R) / 2
Δθ = (Δs_R - Δs_L) / L
x_{k+1} = x_k + Δs·cos(θ_k + Δθ/2)
y_{k+1} = y_k + Δs·sin(θ_k + Δθ/2)
θ_{k+1} = θ_k + Δθ其中 L 为轮距。轮式里程计频率高、短时平滑，但轮子打滑、地面松软、轮胎磨损和标定误差都会造成累积漂移，属于典型的“相对定位”。
惯性测量单元由陀螺仪和加速度计组成，陀螺仪测量角速度，加速度计测量比力。对角速度积分得到姿态，对加速度二次积分得到位移。IMU 输出频率可达 100～1000 Hz，短时动态响应极好，但积分会放大零偏和噪声，姿态和位置误差随时间快速增长，因此通常与轮式里程计组合成轮式惯性里程计。
GNSS通过接收多颗卫星的伪距和载波相位，解算接收机在地心坐标系中的位置。单点定位精度约米级，RTK 差分可到厘米级。GNSS 提供绝对位置、无累积漂移，但依赖天空视野，城市峡谷、室内、隧道和树荫下容易失锁或多路径，输出频率通常只有 1～20 Hz。
激光雷达定位通过扫描环境获得点云，与先验地图或上一帧点云做配准（如 ICP、NDT、AMCL），估计机器人位姿。它直接感知几何结构，精度高、不受光照影响，适合室内和结构化环境；但雨雪雾、玻璃、长走廊和动态人群会降低配准质量，且成本与数据量较大。
视觉定位用相机提取特征点、描述子或直接法光度误差，通过重投影误差优化位姿，也可结合 IMU 构成视觉惯性里程计（VIO）。视觉信息丰富、成本低，能识别纹理和语义；但受光照变化、弱纹理、运动模糊和尺度不确定性影响，纯视觉容易漂移。
二、优缺点对比与选型清单
| 定位源 | 输出频率 | 精度特征 | 主要优点 | 主要缺点 |
|---|---|---|---|---|
| 轮式里程计 | 50～200 Hz | 短时厘米级，长时漂移 | 成本低、连续、易集成 | 打滑、标定误差、累积漂移 |
| IMU | 100～1000 Hz | 短时姿态好，位置漂移快 | 高频、抗遮挡、动态响应好 | 零偏、噪声积分、需标定 |
| GNSS | 1～20 Hz | 单点米级，RTK 厘米级 | 绝对位置、无累积漂移 | 依赖天空视野、易多路径 |
| 激光雷达 | 10～40 Hz | 厘米级，地图依赖 | 几何精度高、不受光照影响 | 雨雾玻璃退化、成本高 |
| 视觉定位 | 20～60 Hz | 厘米级，尺度需辅助 | 信息丰富、成本低、可语义 | 光照弱纹理敏感、易漂移 |
三、典型场景与融合流程
以“室外园区巡检 + 室内抓取”的移动操作机器人为例，其定位流程通常分为四步：
- 高频预测：轮式里程计与 IMU 以 100 Hz 以上输出相对运动，保证短时连续。
- 绝对校正：室外用 GNSS RTK 提供全局位置，室内用激光雷达配准或视觉 VIO 提供局部绝对约束。
- 失效检测：监测轮速与 IMU 一致性、GNSS 卫星数与信噪比、激光配准残差、视觉特征数量，判断某源是否降权或剔除。
- 统一输出：融合后发布 map→odom→base_link 变换，供导航与机械臂操作共用。
# 典型融合节点订阅与发布关系
订阅：/wheel_odom /imu/data /gnss/fix /scan /camera/odom
发布：/odom /tf (map→odom→base_link)
参数：odom_freq: 100 gnss_weight: 0.3 lidar_weight: 0.6四、常见误区与复盘
复盘时应重点检查：各传感器时间戳是否同步、外参是否经过标定、失效检测阈值是否合理、融合权重是否随环境自适应。只有把“高频相对定位”和“低频绝对定位”组织成互补关系，移动平台才能在室内外连续场景中为操作融合提供稳定可靠的位姿基础。