EEDrone開源四旋翼從零開始（9）——控制框架對比分析

其實控制是一件很簡單的事情，無非就是狀態監測、控制和輸出，但是控制出來的效果卻是有好有懷，無非就是上面三個步驟的差別。放在四旋翼上，狀態監測就是使用更好的感測器；控制就是使用魯棒性更好的演算法；輸出就是使用延遲更小效率更高的電機。這其中還有個隱藏的優化項，那就是三者間的延遲，從狀態監測到控制間的延遲和從控制到輸出間的延遲，後者由於技術的原因無法提高了，但 是前者卻是存在著優化的空間。

四旋翼感測器數據的讀取到控制時間應該是要越短越好。但是由於操作系統的加入，這方面做得越來越不好了。下面我從CrazyFlie2，APM，PX4，bataflight和匿名領航者來分析對比控制流程。

CrazyFlie2

CrazyFlie2使用的是FreeRTOS操作系統，其控制主要是兩個任務，如下圖所示：

其中SensorTask任務主要是用來獲取感測器數據的，首先是通過xSemaphoreTake函數獲取中斷的信號量，在感測器數據準備好後（不確定是是採用的MPU6500還是BMI160，設置的採樣率為800Hz），通過引腳中斷通知mcu，在中斷響應函數中發送中斷信號量，然後該任務得以運行，首先進入臨界區，然後使用IIC讀取感測器數據，再將感測器數據通過信號量發送出去，最後退出臨界區。

接下來是姿態控制任務，採用的是精確定時1ms的方案，首先通過SensorAcquire函數獲取SensorTask任務發送的感測器的數據，然後通過Madgwick演算法解算出歐拉角，採用PID控制演算法控制姿態，最後通過不同的機架類型（X或+）分配電機的控制量。

CF2的結構非常清晰，但是其中有個非常大的問題，感測器的採樣頻率比控制頻率低，然後感測器信號量是通過信號量傳遞的，這樣就會導致從感測器數據產生到處理會最多導致2ms的延時。

APM

下面來看看APM的框架，APM是沒有使用操作系統的，最多算是使用了調度演算法。它有個scheduler_tasks表，裡面有各個任務的運行周期，但是卻沒有優先順序。有一個頻率是400Hz的快速循環（fast_loop()），姿態控制任務就放在這裡裡面。關於APM的運行流程我這裡貼一張官方的圖，如下：

APM初始化後，進入主循環，主循環運行頻率為400Hz，在任務調度中有各個任務的調度，任務後台一直在讀取感測器，包括：慣性感測器、氣壓計和GPS等；然後將數據進行擴展卡爾曼濾波解算出姿態和位置；然後接收遙控器的控制信息，根據不同的飛行模式進行不同的設置；然後進入位置控制，位置控制為控制的外環，然後根據位置控制的指令進行姿態控制，姿態控制為控制內環；最後根據控制量轉換為電機的控制值。

APM中不同的飛行模式有對應著不同的控制演算法，下面以自穩模式為例來介紹四旋翼的姿態控制流程，程序首先運行Fligth_mode.cpp中的Set_mode()，在該函數中調用Control_stabilize.cpp中的Stabilize_init()，將模式設置為自穩模式，然後在400Hz的主循環中頻率調用Update_flight_mode()，該函數中調用Control_stabilize.cpp中的Stabilize_run()，該函數中調用AC_AttitudeControl.cpp中的input_euler_angle_roll_pitch_euler_rate_yaw()進行角度的控制，同時在主循環中還會調用rate_controller_run()進行角速度控制，最後通過AP_MotorMatrix.cpp文件中的output_to_motors()得出各個電機控制量。

官方說是有個後台在不斷的收集數據，不過我還是沒有發現哪裡有個後台在專門的收集數據，也沒有發現有使用UORB。不過可以確認的是IMU設置的晶元採樣率是最高的8KHz，所以猜測感測器讀取策略是需要時直接讀取，由於採樣率高，並不會造成太多的延遲。如果有哪位小夥伴知道感測器具體是如何讀取的，還請不吝賜教。

PX4

PX4使用了NUTTX操作系統和uORB數據通信，具體框架借用官網的一張圖如下：

主要分為三個部分，一個是感測器數據採集，採用的是高頻率的採集，然後進行數據驗證；之後通過uORB將數據發布，之後就是數據處理與控制了。高頻率的採集可以有效的避免錯誤數據產生干擾，提高系統的容錯性，不過實時性和系統的開銷會受點影響。

Betaflight

Betaflight（cleanflight）是專門為穿越機和小型四旋翼開發的飛控程序，使用CC3D的硬體，麻雀雖小五臟俱全。Betaflight沒有使用操作系統，使用的是程序調度，應該是非搶佔式的，也有優先順序的概念。主要的控制框架分為三個部分：主循環、加速度值採集和姿態解算。如下圖：

Betaflight的主循環採用100Hz循環，最高優先順序。設計的相當巧妙，陀螺儀數據的讀取和控制放在了一起，通常情況下陀螺儀的數據相當於角速度，相當於姿態控制中的微分部分，直接乘以微分係數就可以了，由於微分本來就是用來超前調節的，所以對實時性要求很高，這裡讀取陀螺儀數據後直接控制。至於直接讀取陀螺儀值當做角速度相比於利用角度值進行微分哪個效果好就不得而知了。不過有個不理解的地方，那就是弄了個控制計數，也就是4次主循環才進行一次控制，這樣控制周期就變成了25Hz，這樣的設定很不理解。控制採用自適應積分的PID控制。

加速度計值的採集採用1000Hz，這裡採用高頻率採集主要是用來濾波，不過濾波的效果不得而知。

姿態解算部分也設計的也比較巧妙，將姿態解算另外做了一個線程，姿態角用來做P運算，通常不需要很高的實時性，況且姿態解算本身也不是那麼可靠，看是將其拉到控制外面，確實可以大大的減少其控制延遲，最大程度的減少了延遲。姿態解算採用的Mahong演算法，佔用資源小，速度快。

匿名領航者

匿名領航者是匿名科創目前最新的產品，程序沒有使用操作系統也沒有使用程序調度，而是使用了一個循環，其中設置不同時間標誌，分為1、2、5、10、20和50ms的分組，所有的任務都放到這些組裡面，關於控制的主要任務如下：

其控制分為內環姿態控制和外環角度控制，內環是2ms一個周期，外環是5ms一個周期，使用Mahong姿態解算，PID控制演算法。

My

分析了這麼多飛控的控制流程，也有了自己的想法，仍然是堅持延遲最低原則，由於以後會有高新能感測器，可能會有兩種形式，一種是做成導航模塊，包含MCU和IMU，直接輸出就是姿態信息，需要中斷引腳通知主MCU；另一種是採用模擬的晶元加高新能ADC，採用主MCU讀取的形式。所以最後的姿態控制流程分為兩種情況如下：

感測器IMU情形下將感測器的數據更新設為最高，精確定時2ms，然後獲取感測器數值、姿態解算、姿態控制和電機控制量分配一氣呵成，這樣做可以最大程度的減少延時。其中的控制主要是姿態控制，位置控制不需要如此高的更新頻率，所以放在另外的任務中。導航IMU由於本身輸出的就是姿態角需要使用引腳中斷來進一步減少延時，所以採用中斷信號量的形式，但是這樣以IMU為時鐘節拍也是有風險的，如果IMU的中斷引腳出了問題，那麼整個系統就不會工作了。

好啦，其主要的控制框架就大概閑聊這些，接下來會對比分析，數據傳輸，文件結構，結構體定義，任務分配，姿態解算演算法，控制方法等。

喜歡這篇文章嗎？立刻分享出去讓更多人知道吧！