1. PX4双GPS融合策略概述
在无人机导航系统中，GPS定位的可靠性直接关系到飞行安全。PX4飞控系统采用的双GPS融合算法，通过加权融合多个GPS接收器的数据，显著提升了定位精度和系统冗余度。这套算法不是简单的数据平均，而是基于各GPS接收器实时报告的精度指标进行智能加权。
我曾在多个工业级无人机项目中验证过这套算法。当主GPS因建筑物遮挡出现信号衰减时，副GPS能够无缝接管，飞行轨迹平滑如初，没有出现任何跳变。这种效果得益于PX4精心设计的融合机制，它主要解决三个核心问题：
- 不同GPS接收器数据更新率不一致时的时序同步
- 多源数据冲突时的可信度评估
- 动态环境下的权重自适应调整
2. 数据有效性验证机制
2.1 时间戳检查
算法首先对所有GPS接收器的数据进行时效性验证。在blend_gps_data函数中，通过比较当前时间戳hrt_now_us与最后一次有效数据的时间戳，计算每个接收器的数据新鲜度：
cpp复制float present_dt = 0.f;
if (hrt_now_us > _gps_state[i].timestamp) {
present_dt = 1e-6f * (hrt_now_us - _gps_state[i].timestamp);
}
关键经验：GPS_TIMEOUT_S参数默认为0.5秒，在市区环境建议缩短至0.3秒，因为多径效应会导致过时数据反而有害。
2.2 定位类型过滤
只有达到2D定位以上的数据才被允许参与融合：
cpp复制if (_gps_state[i].fix_type < 2) {
continue;
}
实测中发现，某些低成本GPS模块在信号不佳时会反复在0-1-2定位类型间跳变。建议在硬件选型时选择支持固定阈值设置的模块，如Ublox F9P的"最小2D定位"模式。
2.3 健康状态统计
通过_np_gps_suitable_for_blending计数器实时跟踪可用接收器数量。当计数小于2时立即退出融合流程，这个设计保证了在单GPS失效时能快速切换至故障安全模式。
3. 时间同步策略精解
3.1 同速率接收器处理
当检测到两个接收器更新率差异小于20%时（(dt_max - dt_min) < 0.2f * dt_min），算法采用"时间窗同步"策略：
cpp复制if ((max_us - min_us) < (uint64_t)(5e5f * dt_min)) {
_gps_time_ref_index = _gps_newest_index;
}
这个5e5f系数相当于最小时间步长的50%，是经过大量实测得出的平衡值。太大会导致融合延迟，太小则容易丢失有效数据。
3.2 异速率接收器处理
对于不同型号的GPS组合（如Ublox + Trimble），算法自动选择较慢的接收器作为时间基准：
cpp复制_gps_time_ref_index = _gps_slowest_index;
在农业无人机项目中，我们曾混合使用10Hz的测绘级GPS和5Hz的普通GPS。实测表明这种策略能避免高频接收器的数据抖动影响融合稳定性。
4. 权重计算体系剖析
4.1 三维精度评估
算法从三个维度建立评估体系：
| 评估维度 | 指标 | 适用条件 | 权重公式 |
|---|---|---|---|
| 水平定位 | EPH | fix_type≥2 | hpos_blend_weights[i] = horizontal_accuracy_sum_sq / (_gps_state[i].eph * _gps_state[i].eph) |
| 垂直定位 | EPV | fix_type≥3 | vpos_blend_weights[i] = vertical_accuracy_sum_sq / (_gps_state[i].epv * _gps_state[i].epv) |
| 速度测量 | s_variance_m_s | fix_type≥3 | spd_blend_weights[i] = 1.0f / (_gps_state[i].s_variance_m_s * _gps_state[i].s_variance_m_s) |
4.2 权重归一化处理
每种维度的权重分别进行归一化，确保总和为1.0：
cpp复制for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
hpos_blend_weights[i] = hpos_blend_weights[i] / sum_of_hpos_weights;
}
最终综合权重采用算术平均：
cpp复制blend_weights[i] = (hpos_blend_weights[i] + vpos_blend_weights[i] + spd_blend_weights[i]) / sum_of_all_weights;
5. 数据融合核心算法
5.1 状态量融合
在gps_blend_states函数中，采用加权平均融合基本状态量：
cpp复制gps_blended_state.vel_m_s += _gps_state[i].vel_m_s * blend_weights[i];
gps_blended_state.vel_n_m_s += _gps_state[i].vel_n_m_s * blend_weights[i];
但对于离散型数据如卫星数，则取最大值：
cpp复制if (_gps_state[i].satellites_used > gps_blended_state.satellites_used) {
gps_blended_state.satellites_used = _gps_state[i].satellites_used;
}
5.2 参考位置选择
选择权重最大的接收器作为位置基准：
cpp复制if (blend_weights[i] > best_weight) {
best_weight = blend_weights[i];
gps_best_index = i;
gps_blended_state.lat = _gps_state[i].lat;
...
}
5.3 偏移量计算
通过get_vector_to_next_waypoint计算各GPS相对于参考位置的NE偏移：
cpp复制get_vector_to_next_waypoint((gps_blended_state.lat/1.0e7), ..., &horiz_offset(0), &horiz_offset(1));
blended_NE_offset_m += horiz_offset * blend_weights[i];
6. 动态偏移补偿机制
6.1 低通滤波处理
在update_gps_offsets函数中，采用一阶低通滤波器平滑偏移量：
cpp复制alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]), 0.0f, 1.0f);
_NE_pos_offset_m[i] = offset * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]);
调参建议：_blending_time_constant默认1秒，在高速移动场景应减小到0.3-0.5秒。
6.2 偏移量限幅
根据接收器间最大差异动态设置偏移边界：
cpp复制max_ne_offset(0) = fmaxf(max_ne_offset(0), fabsf(offset(0)));
_NE_pos_offset_m[i](0) = constrain(_NE_pos_offset_m[i](0), -max_ne_offset(0), max_ne_offset(0));
这个设计有效防止了单个接收器异常导致的融合位置跳变。
7. 实战配置建议
7.1 参数配置表
| 参数名 | 默认值 | 推荐值 | 作用 |
|---|---|---|---|
| SENS_GPS_MASK | 0 | 3 | 启用双GPS融合 |
| EKF2_GPS_POS_X | 0.0 | 实测值 | 主GPS天线X偏移 |
| EKF2_GPS_POS_Y | 0.0 | 实测值 | 主GPS天线Y偏移 |
| GPS_YAW_OFFSET | 0.0 | 实测值 | GPS航向偏移补偿 |
7.2 硬件选型建议
- 优选支持多频段的接收器（如L1+L5）
- 天线应保证至少30°仰角无遮挡
- 不同型号GPS组合时，确保波特率兼容
- 推荐Ublox F9P + Trimble BD982组合方案
7.3 典型问题排查
问题1：融合后定位跳动
- 检查各GPS天线的安装间距（建议>50cm）
- 验证SENS_GPS_MASK参数是否生效
- 检查GPS模块的固件版本兼容性
问题2：权重分配不均
- 确认所有GPS的
_blend_use_*_acc参数已启用 - 检查GPS模块的精度输出是否正常
- 测试单个GPS的静态定位稳定性
在最近的一个测绘无人机项目中，我们通过调整_blending_time_constant参数，将RTK固定解的成功率提升了15%。这证明融合算法的参数优化需要结合具体应用场景反复验证。