4. 农机导航与定位
农机自动驾驶与多机协同作业的第一步，是让车辆在田间“知道自己在哪里、朝向哪里、走得准不准”。单一传感器在农田环境中都难以独立胜任：GNSS 提供绝对位置但易受遮挡与多径影响，RTK 把定位精度提升到厘米级却依赖差分链路，惯性导航（INS/IMU）短时高频但会漂移，视觉里程计（VO）在纹理丰富的作物行间表现良好却缺乏绝对基准。融合定位的本质，就是用不同传感器的互补特性，在时间与空间上互相校正，输出连续、平滑、可信的位姿估计。
一、核心原理：四类传感器的互补关系
GNSS 输出的是全球坐标系下的绝对位置与速度，更新率通常为 1～10 Hz，单点定位误差在米级。RTK 通过基准站与流动站的载波相位差分，把误差压到 2～3 cm，但要求差分数据链路稳定、卫星几何分布良好。IMU 以 100～1000 Hz 输出角速度和加速度，积分后得到短时高精度的姿态与位移增量，但零偏和噪声会随时间累积成漂移。视觉里程计利用相机连续帧的特征匹配或光流，估计相对运动，在作物行、垄沟、地头等有纹理场景中可提供 10～30 Hz 的相对位姿，但受光照、尘土、阴影和尺度不确定性影响。
融合定位的核心不是简单叠加，而是建立统一的误差状态模型，用滤波器或优化方法把各传感器的观测统一到同一状态向量中。常用状态量包括位置、速度、姿态四元数、IMU 零偏、GNSS 接收机钟差等。GNSS/RTK 提供绝对观测，IMU 提供预测，视觉提供相对约束，三者通过卡尔曼滤波或因子图优化实现松耦合或紧耦合。
二、方法流程：从传感器到可用位姿
工程上通常按以下步骤组织融合定位流程：
- 时间同步与坐标统一。用 PPS 秒脉冲或 PTP 协议对齐 GNSS、IMU、相机和轮速计的时间戳，并把所有观测统一到车体坐标系和当地 ENU 坐标系。时间偏差 10 ms 在 3 m/s 车速下就会带来 3 cm 的位置误差，不可忽略。
- IMU 预积分与状态预测。对 IMU 角速度和加速度做预积分，得到相邻关键帧之间的相对旋转、速度和位移增量，作为滤波器的预测步。
- RTK 观测更新。当 RTK 处于固定解时，用厘米级位置观测更新状态；浮点解或单点解时降低权重或仅用于水平约束。
- 视觉相对约束。提取作物行、垄沟或地面特征，估计帧间相对运动，作为位姿约束加入优化；在 GNSS 失锁时，视觉与 IMU 组合可维持短时连续定位。
- 异常检测与鲁棒估计。用卡方检验、新息序列或 RANSAC 剔除多径、误匹配和轮速打滑带来的异常观测。
- 输出与置信度评估。输出位姿及其协方差，供上层路径跟踪和调度模块判断是否可继续作业。
三、典型场景与参数配置
以东北玉米地播种作业为例，车辆沿垄沟直线行驶，地头需要频繁转向。GNSS/RTK 天线安装在车顶，IMU 固定在车体几何中心附近，相机朝前下方安装以观察作物行。融合参数可按以下配置起步：
# 融合定位参数示例（YAML 风格）
gnss:
update_rate: 10 # Hz
rtk_fixed_sigma: 0.02 # m，固定解位置标准差
rtk_float_sigma: 0.35 # m，浮点解位置标准差
imu:
rate: 200 # Hz
accel_noise: 0.02 # m/s^2
gyro_noise: 0.001 # rad/s
vision:
rate: 20 # Hz
feature: crop_row # 作物行特征
max_reproj_error: 2.0 # 像素
fusion:
method: eskf # 误差状态卡尔曼滤波
outlier_chi2: 7.81 # 卡方阈值，95% 置信
max_gnss_gap: 2.0 # s，超过则仅靠 IMU+VO 递推下表对比四类传感器在农田环境中的关键特性，便于选型与权重设计：
| 传感器 | 输出 | 更新率 | 农田优势 | 主要局限 |
|---|---|---|---|---|
| GNSS | 绝对位置、速度 | 1～10 Hz | 全局无漂移 | 树荫、多径、遮挡 |
| RTK | 厘米级绝对位置 | 1～20 Hz | 直线作业精度高 | 依赖差分链路与基站 |
| IMU/INS | 角速度、加速度、姿态 | 100～1000 Hz | 高频、短时连续 | 零偏漂移，需外部校正 |
| 视觉里程计 | 相对位姿、尺度 | 10～30 Hz | 作物行纹理丰富 | 光照、尘土、阴影敏感 |
四、常见误区与复盘要点
误区一：认为 RTK 固定解就等于定位可靠。固定解只说明整周模糊度固定，若天线被遮挡或多径严重，位置仍可能跳变。应结合新息检验和 IMU 短时递推判断是否可信。
误区二：忽略时间同步。GNSS、IMU、相机时间戳未对齐时，融合结果会出现系统性滞后，表现为直线行驶时轨迹“画龙”。
误区三：视觉里程计直接输出绝对位置。VO 本质是相对测量，尺度不确定，必须与 IMU 或轮速联合估计尺度，不能单独作为绝对定位源。
复盘建议：每次作业后回放融合轨迹与原始观测，重点检查三类事件——RTK 失锁时段、地头转向时段、尘土或强光时段。记录这些时段的位姿协方差和实际偏差，用于调整观测噪声权重和异常检测阈值。若发现地头转向时航向误差明显增大，可提高 IMU 陀螺零偏估计的收敛速度，或引入轮速差分的航向约束。
融合定位的最终目标不是追求某一时刻的最高精度，而是在整个作业季、各种天气与地块条件下，持续输出可信、连续、可解释的位姿。只有把 GNSS、RTK、惯性导航和视觉里程计放在统一的误差框架下协同工作，农机才能在播种、施肥、喷药和收获等环节中稳定沿预定路径行驶，并为多机协同调度提供可靠的时空基准。