无人机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<int>(i) == pre_check_best) {

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中级视频教程讨论的内容，如果想观看完整的视频教程和源代码，请在我们微信公众号回复。 

---

技术发展的日新月异，阿木实验室将紧跟技术的脚步，不断把无人机行业最新的技术和硬件推荐给大家。看到经过我们培训的学员在技术上突飞猛进，是我们培训最大的价值。如果你在无人机行业，就请关注我们的公众号：阿木实验室，我们将持续发布无人机行业最有价值的信息和技术。