无人机PX4固件—传感器多冗余机制测评讨论
在开源无人机领域，传感器多冗余一直是PIXHAWK这款飞控的有别于其他开源飞控的特色和硬件成比较高的地方。我们从PIXHAWK控制板的硬件和软件两方面结合LOG日志来分析它的传感器多冗余机制。
MPU6000 3轴加速度和3轴陀螺仪
L3GD20 3轴陀螺仪
LSM303D 3轴加速度计和3轴磁力计
HMC5883 3轴磁力计
MPU9250 3轴加速度 3轴磁力计 3轴陀螺仪（最新版v2飞控才有）
飞控的传感器多冗余通过以上的传感器的组合，就构成了2个IMU。新版的V2在加入了一个MPU9250，就构成了3 IMU的组合。
关键词：sensor_combined，投票机制，confidence。
在函数中sensors.cpp是处理多冗余的基本函数。
MPU6000.cpp L3GD20.cpp
HMC5883.cpp LSM303D.cpp
这几个驱动函数发布的
sensor_gyro
sensor_mag
Sensor_accel ，uorb数据。
由sensors.cpp订阅，由VotedSensorsUpdate类做投票选择出2个(或者3个)传感器里面最可信的那一组数据，给EKF2做姿态解算和位置解算。
上面的传感器不断的发送出2组Accel/Gyro/Mag数据，投票机制选择出最可信的一组传感器数据（VotedSensorsUpdate）。这样就做到了多冗余机制。所以后来的V2版本的固件添加了一个MPU9250，那么就是3冗余的IMU了。换句话说你添加一个IMU，做4冗余的飞控也是同理了。
但是笔者以为还有一种多冗余IMU机制，就是每个单独的Accel/Gyro/Mag都去做EKF滤波，得出的IMU数据在来投票。这样似乎更为合理些，因为EKF2的滤波运算，计算量大，有计算崩溃的风险，EKF2解算崩溃，无论有几个IMU冗余，都会导致飞机失控。如果是多个EKF2独立解算，那么EKF2解算崩溃的几率将会小很多。但是这样一来目前版本的硬件的运算能力不足以支撑多个EKF2的运算。从官方的迹象来看，这种独立的EFK2 解算将会是以后的方向。
投票机制的核心算法：
在Firmware/src/lib/ecl/validation这个文件夹里面是投票机制的核心，摘抄代码如下：
while (next != nullptr) {
float confidence = next->confidence(timestamp);
if (static_cast
pre_check_prio = next->priority();
pre_check_confidence = confidence;//置信区间
}
/*
* Switch if:
* 1) the confidence is higher and priority is equal or higher
* 2) the confidence is no less than 1% different and the priority is higher
*/
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)) {
max_index = i;
max_confidence = confidence;
max_priority = next->priority();
best = next;
}
next = next->sibling();
i++;
}
/* the current best sensor is not matching the previous best sensor,
* or the only sensor went bad */
if (max_index != _curr_best || ((max_confidence < FLT_EPSILON) && (_curr_best >= 0))) {
bool true_failsafe = true;
/* check whether the switch was a failsafe or preferring a higher priority sensor */
if (pre_check_prio != -1 && pre_check_prio < max_priority &&
fabsf(pre_check_confidence - max_confidence) < 0.1f) {
/* this is not a failover */
true_failsafe = false;
/* reset error flags, this is likely a hotplug sensor coming online late */
best->reset_state();
}
/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
if (_curr_best < 0) {
_prev_best = max_index;
} else {
/* we were initialized before, this is a real failsafe */
_prev_best = pre_check_best;
if (true_failsafe) {
_toggle_count++;
/* if this is the first time, log when we failed */
if (_first_failover_time == 0) {
_first_failover_time = timestamp;
}
}
}
/* for all cases we want to keep a record of the best index */
_curr_best = max_index;
}
*index = max_index;
return (best) ? best->value() : nullptr;
笔者搜了很多资料也找不到具体的算法采用，感觉是用到置信区间相关的概率统计理论。
只能通过现象推断结果，如果谁对这个有研究，还希望赐教。我们做如下实验，我们用软磁或者硬磁去干扰外置的磁罗盘，分别在log日志里面记录下外置磁罗盘的数据，在记录下经过投票机制，投票得出的数据。再和真实的地理磁罗盘数据做比对。看投票机制得出的数据是不是可信的。从一个方面验证下这个投票机制的数据抗干扰能力怎么样。有如下工作要完成：
1 修改LOG日志，单独记录外置磁罗盘HMC5883的数据和303D的磁罗盘数据
2 干扰或者切断HMC5883的数据，干扰内置磁罗盘数据
3 外置磁罗盘数据和投票机制的数据和303D磁罗盘数据做比较，分析其抗干扰性能
采用两种方式干扰和切断了磁罗盘数据，来分析LOG日志，判断PX4的多冗余策略是否有效。
如果有效的传感器多冗余策略有如下特点：
1 如果一路外置磁罗盘传感器数据完全失效，内置的磁罗盘数据还是可以工作，系统还是正常工作。
2 如果一路外置的磁罗盘数据受到干扰，内置的没有受到干扰的磁罗盘数据还是正常工作，系统的航向数据没有受到大的干扰。
实际测试的LOG日志如下：
我们可以直观的看到干扰外置磁罗盘和干扰内置磁罗盘，航向数据均会受到很大的干扰，即便外置磁罗盘架高，内置磁罗盘受到干扰，航向数据也会受到一定程度的干扰。但是有些效果的是，我们直接拔掉了外置磁罗盘，但是系统的磁罗盘数据还是有的。
我们可以推测一下，其他陀螺仪，加速度计等传感器的多冗余策略，和磁罗盘采用的一样的算法，也是类似的效果。只能够应对传感器数据完全失效的情况，不能应对传感器数据干扰的情况。
那就是说PX4固件的多冗余策略的抗干扰性能，不是非常好，有很大的改进空间，我感觉这是一个比较好的研究课题和方向。
注：这是我们阿木实验室PX4中级视频教程讨论的内容，如果想观看完整的视频教程和源代码，请在我们微信公众号回复。
技术发展的日新月异，阿木实验室将紧跟技术的脚步，不断把无人机行业最新的技术和硬件推荐给大家。看到经过我们培训的学员在技术上突飞猛进，是我们培训最大的价值。如果你在无人机行业，就请关注我们的公众号：阿木实验室，我们将持续发布无人机行业最有价值的信息和技术。