無人機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中級視訊教程討論的內容，如果想觀看完整的視訊教程和原始碼，請在我們微信公眾號回覆。
技術發展的日新月異，阿木實驗室將緊跟技術的腳步，不斷把無人機行業最新的技術和硬體推薦給大家。看到經過我們培訓的學員在技術上突飛猛進，是我們培訓最大的價值。如果你在無人機行業，就請關注我們的公眾號：阿木實驗室，我們將持續釋出無人機行業最有價值的資訊和技術。