基于STM32的小型四軸飛行器編隊控制系統設計

陳韓悅，劉曉航，王品智，夏昊鵬，賴曉晨，惠煌

（大連理工大學軟件學院，大連116000）

0 引言

相較于傳統載人飛行器，無人機具有體積較小、動作靈活、適用面廣泛、成本低廉等優勢，且因其無人駕駛的特性，可以由控制人員進行遠程操作，適用于執行高風險的特殊任務，能夠有效保障工作人員的生命安全，無論在民用還是軍用領域都越來越受到人們的青睞[1]。

無人機群相較于單架無人機，具有更大的視角，能夠全方位地觀察目標動向，同時針對硬件故障、無線通信受阻、機體動力源失效、環境突變等突發情況具有更高的適應性和容錯率。但無人機群協同控制相較于單架無人機的飛行控制，具有更多的理論和工程難點，如無人機間的通信系統建設、機群隊形的控制保持等。同時，針對無人機數量的增多，如何有效控制成本，也是需要考慮的問題。

1 研究現狀

針對不同規模、不同形式的網絡拓撲結構，應采用不同的編隊控制策略和通信方式[2]。當前對于無人機群編隊控制領域的研究，主要側重于對控制方法的研究，而不過多涉及對無線網絡的拓撲結構研究或通信時延問題的探討。在通信研究領域，對無人機群自組網絡的研究很少涉及對無人機編隊控制策略的需求分析。因此在對無人機群自組網絡的通信技術方面的研究亟待深入。

本文將基于STM32為核心的小型四軸飛行器，利用PID控制技術、2.4G射頻通信技術、地磁校準算法，設計一款四軸飛行器編隊控制系統，實現四軸飛行器的小規模編隊飛行。飛行器能夠實現基本的飛行功能，在空間中穩定懸停、移動和轉向；若干架飛行器能夠組成“主-從”編隊；在飛行過程中，飛行編隊能夠利用環境磁場實時校準統一航向，維持隊形的穩定。

2 研究現狀

2.1 系統結構

為實現四軸飛行器的編隊控制，需要實現無人機飛行控制、飛行器間通信自組網絡以及機群隊形保持等基本模塊。系統整體結構如圖1所示。

圖1 系統整體結構圖

2.2 無人機控制

在無人機控制部分，如圖2所示，系統主要使用姿態解算算法以及閉環控制算法，有效實現了單架無人機飛行過程中的快速自穩，使無人機擁有向期望坐標飛行并在目標位置保持穩定的能力。

圖2 無人機控制模塊結構圖

2.3 通信自組網絡

如圖3所示，在無人機自組網絡中，本項目設計了集中式的網絡拓撲結構。使用了2.4G無線通信模塊nRF24l01及nRF51822的雙向鏈通信方式，實現了以遙控器為控制中心，集中控制無人機群的控制策略。

圖3 無人機網絡拓撲結構圖

2.4 機群隊形保持

在無人機群隊形的保持方面，系統主要利用無人機飛行控制模塊中的空間坐標PID控制，并利用地磁特征對航偏角進行實時修正。本模塊結構如圖4所示。

圖4 機群隊形保持模塊結構圖

3 系統實現

3.1 無人機飛行控制方法

（1）姿態解算

無人機的微型控制單元通過IIC總線讀取九軸傳感器MPU9250的采樣數據。通過IIR低通濾波后，對加速度計的原始數據進行處理，并對陀螺儀的原始數據進行偏置調整。最后將二者處理后的數據融合，得到無人機的姿態角。其中，為減輕歐拉角帶來的正余弦計算負荷，以及歐拉角存在的GimballLock問題，采用四元數形式進行姿態計算，在最后將姿態角的四元數形式轉換為歐拉角形式。

（2）角度和角速度串級PID控制

PID閉環控制主要分為三個部分：比例控制、積分控制、微分控制，其數學公式為：

其中Kp為比例系數，Ki為積分系數，Kd為微分系數為控制量期望值與實際值的誤差。在此定義PID算法的接口函數返回值為經過本周期更新后的輸出量，傳入的參數包含相應PID控制器的比例、積分、微分系數，其設定需要根據四軸飛行效果進行調節，同時還傳入了控制量的期望值與實際值的誤差。

在此基礎上，設計角度環PID和角速度環PID控制方法，其邏輯結構如圖5所示。

圖5 串級PID控制方法

角度環PID控制器的輸入變量為角度期望值以及測量角度值，其中期望角度來自遙控器發送的控制信號，測量角度來自姿態解算得到的無人機姿態，控制器輸出信號為期望的無人機旋轉角速度。

角速度環PID控制器的輸入為角度環PID控制器輸出的期望角速度和實際角速度值，實際角速度值可由陀螺儀進行測量。

（3）空間坐標PID控制

對無人機的三維坐標控制，分為距地高度控制與平面XY坐標控制。PID控制方法的輸出量為XYZ三方向上的期望運動速度，通過特定的轉換方法，可以將其轉化為無人機的期望橫滾角、期望俯仰角以及油門增量。其轉換方法的代碼實現如下所示，其中attitude為無人機的期望姿態角，thrustRaw為無人機的油門增量，setpoint為期望坐標點：

（4）姿態控制量和油門值整合

本設計中的無人機飛行模式采用X型結構，其中電機轉向與飛行器姿態正方向如圖6所示，其中箭頭方向表示姿態角的正方向。

圖6 電機轉向與姿態正方向

每個電機的控制量，由橫滾角控制輸出量、俯仰角控制輸出量、航向角控制輸出量、基礎油門控制輸出量共同決定。根據圖6所描述，電機轉速變化對無人機的飛行影響如表1所示。

表1 電機轉速對無人機飛行影響

由此可以計算出對各個電機的控制量，計算方法如下：

其中control為姿態控制量的結構體，它的thrust元素為飛機定高油門基礎值和油門增量的和，roll、pitch、yaw為飛機的期望姿態角。

3.2 無人機群自組網絡實現

（1）無人機群的自組網配置

本設計采用2.4G射頻通信方式進行遙控器與無人機之間的信息交互。采用集中控制的方法，且為避免信號沖突采用了異步分時復用的通信方案。遙控器作為發送端，為控制中心，在每個周期對編隊中的無人機輪詢地進行無線數據交互。在信宿區分方案上，為編隊內各架無人機配置唯一通信地址。

在無線通信的配置上，無人機以及遙控器統一設置無線通信的數據傳輸率為250kbps，無線通信的通信頻段為信道2。在通信地址的配置上，各架無人機需要設置唯一的通信地址，其地址配置與拓撲結構如圖7所示。

圖7 無人機組網拓撲結構

當nRF24l01工作在Enhanced ShockBurst的發送模式時，僅使用通道0作為接收通道，且需要保持發送地址與通道0的接收地址一致，在發送一個無線報文后，要求在一定時間范圍內，收到接收設備發出的應答信號，若在規定時間內未收到確認，則自動重發，若到達重發次數上限仍未接收到應答信號，則認為該報文發生丟失。在對nRF24l01的配置中，需設置其應答接收時間為1500us，重發次數上限為3次。

經過以上工作即可完成對無人機編隊組網的無線通信配置。

（2）無線通信程序框架

針對地面控制中心，其無線通信的工作流程可概括為：

①發送地址配置：針對編隊中的指定無人機，將發送地址設置為無人機的通信地址。

②獲取數據幀：從控制信號隊列中獲取數據幀，如果信號隊列為空，則獲取空數據幀。

③數據幀發送并等待應答信號：將②中獲取的數據幀送入nRF24l01的發送寄存器中，并使能無線傳輸，之后等待應答信號，若收到應答信號，則將該數據幀放入接收數據隊列。

④發送判斷：根據是否成功收到應答信號，改變全局標識符。

⑤指定下一個無人機。

3.3 無人機協同編隊飛行控制方法實現

采用長機-僚機飛行策略，長機的坐標包括：距地相對高度、機體坐標系上的X坐標、Y坐標，其可由氣壓計、光流傳感器采樣獲得。不考慮通信阻塞、數據丟包等問題，長機以1ms為周期，向遙控器發送自身的坐標數據，數據幀通過幀頭的功能字字段加以區分，遙控器接收到長機的坐標數據后，計算僚機期望的飛行軌跡，并依次向所有僚機進行轉發。僚機的閉環飛行控制方法如圖8所示。

圖8 僚機的閉環飛行控制

3.4 地磁特征修正航偏角的實現

（1）通過磁力傳感器數據計算航向

地磁場在一定范圍內可以看作是空間中的勻強磁場，因此可以作為測算的基準。下面引入兩個坐標系：磁地理坐標系（m系）及飛行器坐標系（b系）。其中m系以水平面為XY平面，以磁場在水平面上的投影方向為X軸正方向。b系以飛行器機身平面為XY平面，機頭朝向為X軸正方向。

當m系與b系重合時，在飛行器上安裝磁力傳感器，使得磁傳感器的測量值Mb如式（2）所示：

其中MN為當地磁場的水平分量大小，MD為當地磁場的垂直分量大小[3]。

此后，無論飛行器處于任何姿態，磁傳感器的輸出均可以表示為：

根據三角函數知識，利用飛行器的橫滾角γ和俯仰角θ，可得：

從而得到地磁場矢量在X軸和Y軸的分量為：

則磁航偏角為：

ψm為b系X軸正方向在m系XY平面上的投影與m系X軸正方向的夾角。由于地磁場在一定范圍內可以視為勻強磁場，因此只要編隊內各飛行器ψm值保持一致，即可維持航向角的統一。

（2）磁場校準算法

由3.4（1）中的描述可知，若b系XY平面與m系XY平面平行，且m系X軸與b系X軸正向平行時，磁力數據應為式（2）所示，顯然在二者反向平行時，磁力數據為：

但由于磁場數據會受到外界的干擾，而其中最主要的是飛行器自身電子器件所產生的恒定磁場對其干擾。這一點在圖像上表現為球體位置的平移和形狀的拉伸，即實際數據樣本在采樣空間中所顯示的圖像為中心位于空間中某一點的橢球。故我們不能將磁力計所獲取的數據直接用于計算磁航偏角ψm。

因此，需要求出該橢球的表達式，以便對磁力計數據進行修正。本項目采用最小二乘法進行橢球擬合。

通過理論分析和實驗證明，磁場橢球主要經歷了拉伸和平移變換，而沒有經歷旋轉變換，因此可以將其表示為：

其中(x0,y0,z0)為橢球的中心，a,b,c為三個半軸的長度。

將式（8）展開，可以得到：

將一組實際測量的磁場數據帶入到上式：

顯然，若a=0則表明樣本點落在橢球表面，若a≠0則表示存在偏差。由最小二乘法思想，我們將等式兩邊分別平方進行累加得用來表示總誤差。為使Q取極小值，令Q對A,B,C,D,E,F的偏導數均為0，可以得到如下方程組：

將方程組（11）整理成矩陣形式：

利用高斯消元法，將矩陣轉換成上三角形式，回代后即可求解出A,B,C,D,E,F的值。由此，便獲得了該橢球方程的一般式，為了對磁場傳感器的數據進行修正，需要將其化為形如式（8）所示的標準式。在得到及a,b,c后，便可將原始數據映射到標準球面上，從而實現磁航偏角ψm的計算。

4 結語

基于STM32的小型四軸飛行器編隊控制系統，提供了四軸飛行器的飛行控制、飛行器間通信組網以及機群的隊形保持等基本模塊，實現了小型四軸飛行器的編隊飛行。系統所設計的利用地磁場校準航向角的思路具有一定的創新性。系統所涉及的硬件資源較為精簡，有利于四軸飛行器編隊的小型化和低成本化，具有一定的現實意義和實用價值。