PX4从放弃到精通（二十九）：传感器冗余机制中的置信度与优先级博弈-编程阁

1. 传感器冗余机制的核心价值

飞行控制系统中的传感器就像人体的感官系统，任何一个传感器的失效都可能导致灾难性后果。我在实际项目中遇到过多次因传感器故障引发的意外情况，比如磁力计受干扰导致无人机失控，或是气压计堵塞引发高度测量错误。PX4的传感器冗余机制就是为了解决这类问题而设计的。

这套机制的核心思想很简单：不要把所有鸡蛋放在一个篮子里。通过部署多个同类型传感器，系统可以在某个传感器失效时自动切换到备用传感器。但实现起来却有不少门道，关键在于如何判断哪个传感器更可靠，以及在什么时机进行切换。

举个例子，假设你的无人机装了三套IMU（惯性测量单元），当主IMU出现问题时，系统需要快速判断是切换到IMU2还是IMU3。这个决策过程就涉及到两个关键指标：置信度（confidence）和优先级（priority）。置信度反映传感器当前数据的可信程度，优先级则代表传感器的固有等级。

2. 置信度的计算逻辑

2.1 错误密度与均方根误差

置信度的计算是PX4传感器冗余机制中最精妙的部分。在代码中可以看到，它主要依赖两个指标：错误密度（error_density）和均方根误差（RMS）。

错误密度有点像传感器的"健康度报告卡"。每次传感器读数时，系统会检查是否有错误计数（error_count）。这个错误计数通常由底层驱动提供，反映硬件层面的读取异常。比如I2C通信失败、数据校验错误等。错误密度会根据这些错误计数动态调整：

if (error_count_in > _error_count) { _error_density += (error_count_in - _error_count); } else if (_error_density > 0) { _error_density--; }

均方根误差则评估传感器数据的稳定性。它通过统计一段时间内数据的波动情况，计算出一个反映数据离散程度的指标。在代码中，这个计算过程使用了在线算法，避免存储大量历史数据：

float lp_val = val[i] - _lp[i]; float delta_val = lp_val - _mean[i]; _mean[i] += delta_val / _event_count; _M2[i] += delta_val * (lp_val - _mean[i]); _rms[i] = sqrtf(_M2[i] / (_event_count - 1));

2.2 置信度公式解析

最终置信度的计算公式非常简单：

ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW);

这里的ERROR_DENSITY_WINDOW是个固定值100，相当于把错误密度归一化到0-100的范围。当错误密度达到100时，置信度就降为0，表示传感器完全不可信。

但实际代码中还有几个边界条件检查：

数据超时（长时间没有新数据）
数据僵死（连续多次读数完全相同）
错误计数超过阈值

这些情况都会直接导致置信度归零。我在调试时发现，数据僵死检测特别有用，能及时发现传感器卡死的情况。

3. 优先级的设定与调整

3.1 校准与优先级初始化

传感器的优先级不是在代码中硬编码的，而是通过校准过程确定的。在PX4中，每完成一次传感器校准，系统会为同类传感器分配一个优先级值，存储在参数系统中。

查看parametersUpdate函数可以看到，优先级是从校准参数中读取的：

_accel.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("ACC", "PRIO", accel_cal_index); _gyro.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("GYRO", "PRIO", gyro_cal_index);

优先级的范围通常是1-100，数值越大优先级越高。在实际应用中，我会给质量更好的传感器设置更高的优先级。比如某款无人机的IMU模块经过振动测试表现优异，我就会在校准后手动将其优先级调高。

3.2 动态优先级调整

有趣的是，PX4中的优先级并不是完全静态的。代码中有一个细节：

int priority_change = _accel.priority_configured[uorb_index] - accel_priority_old; _accel.priority[uorb_index] = math::constrain(_accel.priority[uorb_index] + priority_change, static_cast<int32_t>(1), static_cast<int32_t>(100));

这意味着如果用户在参数中修改了配置优先级，运行时的实际优先级也会相应调整。这种设计提供了很大的灵活性，允许在飞行中根据实际情况调整传感器的重要性。

4. 最优传感器的选择策略

4.1 决策逻辑详解

在get_best函数中，PX4使用了一套精心设计的决策逻辑来选择最优传感器。这个逻辑可以概括为三种情况：

紧急切换：当当前最佳传感器的置信度低于最小阈值（MIN_REGULAR_CONFIDENCE），而另一个传感器达到阈值时，不考虑优先级直接切换。这确保了系统在主要传感器失效时能快速恢复。
质量优先：当候选传感器置信度更高，且优先级不低于当前最佳传感器时，进行切换。这是最常见的切换情况。
优先级决胜：当两个传感器置信度相差不足1%，但候选传感器优先级更高时，选择优先级高的。

用代码表示就是：

if ((((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) || (confidence > max_confidence && (next->priority() >= max_priority)) || (fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority))) && (confidence > 0.0f)) { // 切换最佳传感器 }

4.2 实际应用中的权衡

在实际飞行测试中，我发现这套机制大多数情况下表现良好，但也有些值得注意的地方：

优先级设置不当的风险：如果人为给某个传感器设置了过高的优先级，即使它数据质量下降，系统也可能不会及时切换到更好的传感器。我曾遇到过因为优先级设置不当导致无人机使用漂移的陀螺仪数据的情况。
切换抖动问题：当两个传感器置信度接近时，可能会产生频繁切换。PX4通过设置1%的置信度差值阈值来缓解这个问题。
错误检测的局限性：当前的错误检测主要依赖硬件报错和数据统计特性，对于某些渐进式失效（如温度漂移）可能不够敏感。