基于STM32的小型四軸飛行器編隊(duì)控制系統(tǒng)設(shè)計(jì)

陳韓悅，劉曉航，王品智，夏昊鵬，賴(lài)曉晨，惠煌

（大連理工大學(xué)軟件學(xué)院，大連116000）

0 引言

相較于傳統(tǒng)載人飛行器，無(wú)人機(jī)具有體積較小、動(dòng)作靈活、適用面廣泛、成本低廉等優(yōu)勢(shì)，且因其無(wú)人駕駛的特性，可以由控制人員進(jìn)行遠(yuǎn)程操作，適用于執(zhí)行高風(fēng)險(xiǎn)的特殊任務(wù)，能夠有效保障工作人員的生命安全，無(wú)論在民用還是軍用領(lǐng)域都越來(lái)越受到人們的青睞[1]。

無(wú)人機(jī)群相較于單架無(wú)人機(jī)，具有更大的視角，能夠全方位地觀(guān)察目標(biāo)動(dòng)向，同時(shí)針對(duì)硬件故障、無(wú)線(xiàn)通信受阻、機(jī)體動(dòng)力源失效、環(huán)境突變等突發(fā)情況具有更高的適應(yīng)性和容錯(cuò)率。但無(wú)人機(jī)群協(xié)同控制相較于單架無(wú)人機(jī)的飛行控制，具有更多的理論和工程難點(diǎn)，如無(wú)人機(jī)間的通信系統(tǒng)建設(shè)、機(jī)群隊(duì)形的控制保持等。同時(shí)，針對(duì)無(wú)人機(jī)數(shù)量的增多，如何有效控制成本，也是需要考慮的問(wèn)題。

1 研究現(xiàn)狀

針對(duì)不同規(guī)模、不同形式的網(wǎng)絡(luò)拓?fù)浣Y(jié)構(gòu)，應(yīng)采用不同的編隊(duì)控制策略和通信方式[2]。當(dāng)前對(duì)于無(wú)人機(jī)群編隊(duì)控制領(lǐng)域的研究，主要側(cè)重于對(duì)控制方法的研究，而不過(guò)多涉及對(duì)無(wú)線(xiàn)網(wǎng)絡(luò)的拓?fù)浣Y(jié)構(gòu)研究或通信時(shí)延問(wèn)題的探討。在通信研究領(lǐng)域，對(duì)無(wú)人機(jī)群自組網(wǎng)絡(luò)的研究很少涉及對(duì)無(wú)人機(jī)編隊(duì)控制策略的需求分析。因此在對(duì)無(wú)人機(jī)群自組網(wǎng)絡(luò)的通信技術(shù)方面的研究亟待深入。

本文將基于STM32為核心的小型四軸飛行器，利用PID控制技術(shù)、2.4G射頻通信技術(shù)、地磁校準(zhǔn)算法，設(shè)計(jì)一款四軸飛行器編隊(duì)控制系統(tǒng)，實(shí)現(xiàn)四軸飛行器的小規(guī)模編隊(duì)飛行。飛行器能夠?qū)崿F(xiàn)基本的飛行功能，在空間中穩(wěn)定懸停、移動(dòng)和轉(zhuǎn)向；若干架飛行器能夠組成“主-從”編隊(duì)；在飛行過(guò)程中，飛行編隊(duì)能夠利用環(huán)境磁場(chǎng)實(shí)時(shí)校準(zhǔn)統(tǒng)一航向，維持隊(duì)形的穩(wěn)定。

2 研究現(xiàn)狀

2.1 系統(tǒng)結(jié)構(gòu)

為實(shí)現(xiàn)四軸飛行器的編隊(duì)控制，需要實(shí)現(xiàn)無(wú)人機(jī)飛行控制、飛行器間通信自組網(wǎng)絡(luò)以及機(jī)群隊(duì)形保持等基本模塊。系統(tǒng)整體結(jié)構(gòu)如圖1所示。

圖1 系統(tǒng)整體結(jié)構(gòu)圖

2.2 無(wú)人機(jī)控制

在無(wú)人機(jī)控制部分，如圖2所示，系統(tǒng)主要使用姿態(tài)解算算法以及閉環(huán)控制算法，有效實(shí)現(xiàn)了單架無(wú)人機(jī)飛行過(guò)程中的快速自穩(wěn)，使無(wú)人機(jī)擁有向期望坐標(biāo)飛行并在目標(biāo)位置保持穩(wěn)定的能力。

圖2 無(wú)人機(jī)控制模塊結(jié)構(gòu)圖

2.3 通信自組網(wǎng)絡(luò)

如圖3所示，在無(wú)人機(jī)自組網(wǎng)絡(luò)中，本項(xiàng)目設(shè)計(jì)了集中式的網(wǎng)絡(luò)拓?fù)浣Y(jié)構(gòu)。使用了2.4G無(wú)線(xiàn)通信模塊nRF24l01及nRF51822的雙向鏈通信方式，實(shí)現(xiàn)了以遙控器為控制中心，集中控制無(wú)人機(jī)群的控制策略。

圖3 無(wú)人機(jī)網(wǎng)絡(luò)拓?fù)浣Y(jié)構(gòu)圖

2.4 機(jī)群隊(duì)形保持

在無(wú)人機(jī)群隊(duì)形的保持方面，系統(tǒng)主要利用無(wú)人機(jī)飛行控制模塊中的空間坐標(biāo)PID控制，并利用地磁特征對(duì)航偏角進(jìn)行實(shí)時(shí)修正。本模塊結(jié)構(gòu)如圖4所示。

圖4 機(jī)群隊(duì)形保持模塊結(jié)構(gòu)圖

3 系統(tǒng)實(shí)現(xiàn)

3.1 無(wú)人機(jī)飛行控制方法

（1）姿態(tài)解算

無(wú)人機(jī)的微型控制單元通過(guò)IIC總線(xiàn)讀取九軸傳感器MPU9250的采樣數(shù)據(jù)。通過(guò)IIR低通濾波后，對(duì)加速度計(jì)的原始數(shù)據(jù)進(jìn)行處理，并對(duì)陀螺儀的原始數(shù)據(jù)進(jìn)行偏置調(diào)整。最后將二者處理后的數(shù)據(jù)融合，得到無(wú)人機(jī)的姿態(tài)角。其中，為減輕歐拉角帶來(lái)的正余弦計(jì)算負(fù)荷，以及歐拉角存在的GimballLock問(wèn)題，采用四元數(shù)形式進(jìn)行姿態(tài)計(jì)算，在最后將姿態(tài)角的四元數(shù)形式轉(zhuǎn)換為歐拉角形式。

（2）角度和角速度串級(jí)PID控制

PID閉環(huán)控制主要分為三個(gè)部分：比例控制、積分控制、微分控制，其數(shù)學(xué)公式為：

其中Kp為比例系數(shù)，Ki為積分系數(shù)，Kd為微分系數(shù)為控制量期望值與實(shí)際值的誤差。在此定義PID算法的接口函數(shù)返回值為經(jīng)過(guò)本周期更新后的輸出量，傳入的參數(shù)包含相應(yīng)PID控制器的比例、積分、微分系數(shù)，其設(shè)定需要根據(jù)四軸飛行效果進(jìn)行調(diào)節(jié)，同時(shí)還傳入了控制量的期望值與實(shí)際值的誤差。

在此基礎(chǔ)上，設(shè)計(jì)角度環(huán)PID和角速度環(huán)PID控制方法，其邏輯結(jié)構(gòu)如圖5所示。

圖5 串級(jí)PID控制方法

角度環(huán)PID控制器的輸入變量為角度期望值以及測(cè)量角度值，其中期望角度來(lái)自遙控器發(fā)送的控制信號(hào)，測(cè)量角度來(lái)自姿態(tài)解算得到的無(wú)人機(jī)姿態(tài)，控制器輸出信號(hào)為期望的無(wú)人機(jī)旋轉(zhuǎn)角速度。

角速度環(huán)PID控制器的輸入為角度環(huán)PID控制器輸出的期望角速度和實(shí)際角速度值，實(shí)際角速度值可由陀螺儀進(jìn)行測(cè)量。

（3）空間坐標(biāo)PID控制

對(duì)無(wú)人機(jī)的三維坐標(biāo)控制，分為距地高度控制與平面XY坐標(biāo)控制。PID控制方法的輸出量為XYZ三方向上的期望運(yùn)動(dòng)速度，通過(guò)特定的轉(zhuǎn)換方法，可以將其轉(zhuǎn)化為無(wú)人機(jī)的期望橫滾角、期望俯仰角以及油門(mén)增量。其轉(zhuǎn)換方法的代碼實(shí)現(xiàn)如下所示，其中attitude為無(wú)人機(jī)的期望姿態(tài)角，thrustRaw為無(wú)人機(jī)的油門(mén)增量，setpoint為期望坐標(biāo)點(diǎn)：

（4）姿態(tài)控制量和油門(mén)值整合

本設(shè)計(jì)中的無(wú)人機(jī)飛行模式采用X型結(jié)構(gòu)，其中電機(jī)轉(zhuǎn)向與飛行器姿態(tài)正方向如圖6所示，其中箭頭方向表示姿態(tài)角的正方向。

圖6 電機(jī)轉(zhuǎn)向與姿態(tài)正方向

每個(gè)電機(jī)的控制量，由橫滾角控制輸出量、俯仰角控制輸出量、航向角控制輸出量、基礎(chǔ)油門(mén)控制輸出量共同決定。根據(jù)圖6所描述，電機(jī)轉(zhuǎn)速變化對(duì)無(wú)人機(jī)的飛行影響如表1所示。

表1 電機(jī)轉(zhuǎn)速對(duì)無(wú)人機(jī)飛行影響

由此可以計(jì)算出對(duì)各個(gè)電機(jī)的控制量，計(jì)算方法如下：

其中control為姿態(tài)控制量的結(jié)構(gòu)體，它的thrust元素為飛機(jī)定高油門(mén)基礎(chǔ)值和油門(mén)增量的和，roll、pitch、yaw為飛機(jī)的期望姿態(tài)角。

3.2 無(wú)人機(jī)群自組網(wǎng)絡(luò)實(shí)現(xiàn)

（1）無(wú)人機(jī)群的自組網(wǎng)配置

本設(shè)計(jì)采用2.4G射頻通信方式進(jìn)行遙控器與無(wú)人機(jī)之間的信息交互。采用集中控制的方法，且為避免信號(hào)沖突采用了異步分時(shí)復(fù)用的通信方案。遙控器作為發(fā)送端，為控制中心，在每個(gè)周期對(duì)編隊(duì)中的無(wú)人機(jī)輪詢(xún)地進(jìn)行無(wú)線(xiàn)數(shù)據(jù)交互。在信宿區(qū)分方案上，為編隊(duì)內(nèi)各架無(wú)人機(jī)配置唯一通信地址。

在無(wú)線(xiàn)通信的配置上，無(wú)人機(jī)以及遙控器統(tǒng)一設(shè)置無(wú)線(xiàn)通信的數(shù)據(jù)傳輸率為250kbps，無(wú)線(xiàn)通信的通信頻段為信道2。在通信地址的配置上，各架無(wú)人機(jī)需要設(shè)置唯一的通信地址，其地址配置與拓?fù)浣Y(jié)構(gòu)如圖7所示。

圖7 無(wú)人機(jī)組網(wǎng)拓?fù)浣Y(jié)構(gòu)

當(dāng)nRF24l01工作在Enhanced ShockBurst的發(fā)送模式時(shí)，僅使用通道0作為接收通道，且需要保持發(fā)送地址與通道0的接收地址一致，在發(fā)送一個(gè)無(wú)線(xiàn)報(bào)文后，要求在一定時(shí)間范圍內(nèi)，收到接收設(shè)備發(fā)出的應(yīng)答信號(hào)，若在規(guī)定時(shí)間內(nèi)未收到確認(rèn)，則自動(dòng)重發(fā)，若到達(dá)重發(fā)次數(shù)上限仍未接收到應(yīng)答信號(hào)，則認(rèn)為該報(bào)文發(fā)生丟失。在對(duì)nRF24l01的配置中，需設(shè)置其應(yīng)答接收時(shí)間為1500us，重發(fā)次數(shù)上限為3次。

經(jīng)過(guò)以上工作即可完成對(duì)無(wú)人機(jī)編隊(duì)組網(wǎng)的無(wú)線(xiàn)通信配置。

（2）無(wú)線(xiàn)通信程序框架

針對(duì)地面控制中心，其無(wú)線(xiàn)通信的工作流程可概括為：

①發(fā)送地址配置：針對(duì)編隊(duì)中的指定無(wú)人機(jī)，將發(fā)送地址設(shè)置為無(wú)人機(jī)的通信地址。

②獲取數(shù)據(jù)幀：從控制信號(hào)隊(duì)列中獲取數(shù)據(jù)幀，如果信號(hào)隊(duì)列為空，則獲取空數(shù)據(jù)幀。

③數(shù)據(jù)幀發(fā)送并等待應(yīng)答信號(hào)：將②中獲取的數(shù)據(jù)幀送入nRF24l01的發(fā)送寄存器中，并使能無(wú)線(xiàn)傳輸，之后等待應(yīng)答信號(hào)，若收到應(yīng)答信號(hào)，則將該數(shù)據(jù)幀放入接收數(shù)據(jù)隊(duì)列。

④發(fā)送判斷：根據(jù)是否成功收到應(yīng)答信號(hào)，改變?nèi)謽?biāo)識(shí)符。

⑤指定下一個(gè)無(wú)人機(jī)。

3.3 無(wú)人機(jī)協(xié)同編隊(duì)飛行控制方法實(shí)現(xiàn)

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

圖8 僚機(jī)的閉環(huán)飛行控制

3.4 地磁特征修正航偏角的實(shí)現(xiàn)

（1）通過(guò)磁力傳感器數(shù)據(jù)計(jì)算航向

地磁場(chǎng)在一定范圍內(nèi)可以看作是空間中的勻強(qiáng)磁場(chǎng)，因此可以作為測(cè)算的基準(zhǔn)。下面引入兩個(gè)坐標(biāo)系：磁地理坐標(biāo)系（m系）及飛行器坐標(biāo)系（b系）。其中m系以水平面為XY平面，以磁場(chǎng)在水平面上的投影方向?yàn)閄軸正方向。b系以飛行器機(jī)身平面為XY平面，機(jī)頭朝向?yàn)閄軸正方向。

當(dāng)m系與b系重合時(shí)，在飛行器上安裝磁力傳感器，使得磁傳感器的測(cè)量值Mb如式（2）所示：

其中MN為當(dāng)?shù)卮艌?chǎng)的水平分量大小，MD為當(dāng)?shù)卮艌?chǎng)的垂直分量大小[3]。

此后，無(wú)論飛行器處于任何姿態(tài)，磁傳感器的輸出均可以表示為：

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

從而得到地磁場(chǎng)矢量在X軸和Y軸的分量為：

則磁航偏角為：

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

（2）磁場(chǎng)校準(zhǔn)算法

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

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

因此，需要求出該橢球的表達(dá)式，以便對(duì)磁力計(jì)數(shù)據(jù)進(jìn)行修正。本項(xiàng)目采用最小二乘法進(jìn)行橢球擬合。

通過(guò)理論分析和實(shí)驗(yàn)證明，磁場(chǎng)橢球主要經(jīng)歷了拉伸和平移變換，而沒(méi)有經(jīng)歷旋轉(zhuǎn)變換，因此可以將其表示為：

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

將式（8）展開(kāi)，可以得到：

將一組實(shí)際測(cè)量的磁場(chǎng)數(shù)據(jù)帶入到上式：

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

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

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

4 結(jié)語(yǔ)

基于STM32的小型四軸飛行器編隊(duì)控制系統(tǒng)，提供了四軸飛行器的飛行控制、飛行器間通信組網(wǎng)以及機(jī)群的隊(duì)形保持等基本模塊，實(shí)現(xiàn)了小型四軸飛行器的編隊(duì)飛行。系統(tǒng)所設(shè)計(jì)的利用地磁場(chǎng)校準(zhǔn)航向角的思路具有一定的創(chuàng)新性。系統(tǒng)所涉及的硬件資源較為精簡(jiǎn)，有利于四軸飛行器編隊(duì)的小型化和低成本化，具有一定的現(xiàn)實(shí)意義和實(shí)用價(jià)值。