基于ADRC控制器的多MC四旋翼系統(tǒng)設(shè)計(jì)

王慧 梁展豪 葉熾鵬 劉威凌 陳倬彬

（廣東海洋大學(xué)電子與信息工程學(xué)院 廣東省湛江市 524088）

隨著如今硬件的性能的不斷提高，各種智能硬件產(chǎn)業(yè)蓬勃發(fā)展，而四旋翼憑借自身獨(dú)特優(yōu)勢(shì)已逐漸滲透到許多的工業(yè)場(chǎng)景以及影視生產(chǎn)的應(yīng)用之中，為人類的工作和生活帶來了極大的便利。

然而，在目前四旋翼系統(tǒng)設(shè)計(jì)的領(lǐng)域，抗擾動(dòng)姿態(tài)控制算法和飛行器上單片機(jī)的運(yùn)算性能限制是亟待優(yōu)化的重大難題，采用多MCU 的四旋翼系統(tǒng)能夠讓飛行器的運(yùn)算能力突破限制，且減少硬件錯(cuò)誤發(fā)生和任務(wù)復(fù)雜時(shí)以至單片機(jī)無法處理的情況發(fā)生。同時(shí)ADRC 控制算法的出現(xiàn)讓控制效果得到的進(jìn)一步提高，將兩種方法結(jié)合，然后將之有效融入四旋翼系統(tǒng)的設(shè)計(jì)開發(fā)之中，能有效解決上述的問題，并且從軟件和硬件層面，提高系統(tǒng)的穩(wěn)定性和可拓展性。這些技術(shù)的結(jié)合使用，將會(huì)是四旋翼在實(shí)用化和智能化的路上效率更高的關(guān)鍵。

本設(shè)計(jì)中用多塊STM32 芯片作為四旋翼系統(tǒng)中的主控芯片，使用ADRC 和PID 的串級(jí)控制實(shí)現(xiàn)定高和姿態(tài)的控制，以及使用光流傳感器識(shí)別的結(jié)果反饋到飛行器的水平控制上，讓其能夠判斷機(jī)體是否在移動(dòng)，并使用Openmv 攝像頭來實(shí)現(xiàn)外界圖像信息處理，以實(shí)現(xiàn)四旋翼系統(tǒng)的跟蹤物體，避障等功能。

在本文中，重點(diǎn)講述系統(tǒng)的軟件部分，姿態(tài)解算使用基于四元數(shù)梯度下降法并且將四旋翼的姿態(tài)解算單獨(dú)由一塊MCU 處理，四旋翼系統(tǒng)穩(wěn)定控制器使用的是串級(jí)ADRC 結(jié)合PID 的控制器，該任務(wù)也同樣交給另外一塊MCU 處理，同時(shí)將系統(tǒng)的人機(jī)交互界面和內(nèi)存管理單獨(dú)由一塊MCU 處理。

1 系統(tǒng)總體方案設(shè)計(jì)

1.1 系統(tǒng)總體結(jié)構(gòu)

基于ADRC 控制器的多MCU 四旋翼系統(tǒng)的總體結(jié)構(gòu)由3 塊MCU、2.4 G 遙控器、光流模塊、Openmv 攝像頭模塊、飛行器姿態(tài)控制器、飛行器高度控制器、飛行器水平位置控制器、地面站上位機(jī)、無刷電機(jī)及電調(diào)等模塊構(gòu)成，三塊MCU 通過內(nèi)部的總線合理分配資源和傳送數(shù)據(jù)，并實(shí)現(xiàn)互相協(xié)助，各模塊之間相互聯(lián)系，相互協(xié)調(diào)，讓整個(gè)系統(tǒng)穩(wěn)定運(yùn)行，使多MCU四旋翼系統(tǒng)能夠穩(wěn)定工作，完成各項(xiàng)要求?？傮w結(jié)構(gòu)如圖1 所示。

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

1.1.1 各核心系統(tǒng)說明

（1）多MCU 核心系統(tǒng)：在過去的大多數(shù)飛行器控制系統(tǒng)中，多數(shù)使用的都是單塊MCU，作為核心處理器，但是單塊芯片的運(yùn)算能力是有限的，一旦隨著飛行器系統(tǒng)的任務(wù)繁雜，整個(gè)系統(tǒng)的穩(wěn)定性也隨之下降，本項(xiàng)目使用的3 塊MCU，作為的整個(gè)系統(tǒng)的核心。其中通過高速度的DMA 總線互相傳輸數(shù)據(jù)。

（2）接收機(jī)模塊：飛行器系統(tǒng)在運(yùn)行過程中，要收到地面站或者遙控器的指令才能正常的運(yùn)行，控制指令由2.4G的接收機(jī)模塊輸入，通過定時(shí)器的捕獲俯仰角、橫滾角、偏航角、油門以及其他特別的輸入信號(hào)，轉(zhuǎn)換數(shù)值信號(hào)。以此傳送到控制器內(nèi)部，完成控制要求。

（3）DMA 總線：使用的DMA 傳輸?shù)奶攸c(diǎn)，不占用CPU 的處理資源，姿態(tài)解算部分的MCU 解算完三軸的各種運(yùn)動(dòng)數(shù)據(jù)后添加頭部和尾部的標(biāo)志幀后，通過總線傳送到人機(jī)交互的MCU 作為數(shù)據(jù)可視化和保存到內(nèi)存卡內(nèi)，同時(shí)也會(huì)傳送到負(fù)責(zé)控制系統(tǒng)的MCU 中。

（4）人機(jī)交互模塊：主要有運(yùn)算能力不高的MCU 去處理，負(fù)責(zé)完成將飛行器系統(tǒng)的整個(gè)交互，如飛行器狀態(tài)顯示、飛行器校準(zhǔn)信息、以及航行數(shù)據(jù)的記錄到內(nèi)存卡中的任務(wù),方便飛行器脫機(jī)調(diào)試,保證飛行器的使用更加人性化與實(shí)用性。

（5）Openmv 攝像頭模塊：這是一款使用H7 系列芯片的攝像頭模組，其強(qiáng)大的運(yùn)算頻率可以實(shí)現(xiàn)許多硬件上的可以運(yùn)行的智能算法和圖像識(shí)別的算法，這個(gè)就可以使飛行器脫離于沉重的路由器以及單獨(dú)在硬件上就可完成的圖像相關(guān)的任務(wù)。

（6）光流模塊：使用的是稀疏光流，用于讀出機(jī)體相對(duì)于東北天坐標(biāo)系的相對(duì)的XY 軸的速度，融合進(jìn)控制系統(tǒng)中，使得飛行器能更好地懸停。

1.2 控制系統(tǒng)硬件設(shè)計(jì)

在硬件系統(tǒng)的設(shè)計(jì)中，電源的穩(wěn)定性是所有東西基本保障，并且對(duì)于四旋翼系統(tǒng)中的姿態(tài)解算所需要用到的傳感器都是對(duì)電源的純凈有著非常高的要求，也就是需要電源的噪聲低，電源的諧波要隔絕開來，同時(shí)對(duì)于整體硬件系統(tǒng)而言，最大輸如電流，紋波抑制大小，電壓波動(dòng)范圍都是非常重要且需要仔細(xì)設(shè)計(jì)參數(shù)，本實(shí)驗(yàn)采取將主控供電和傳感器供電分離得方案，并且做好電源接入處放反接電路，以及自恢復(fù)保險(xiǎn)絲電路，對(duì)核心電路形成更良好得保護(hù)。同時(shí)對(duì)于IMU 模塊，因?yàn)轱w行器系統(tǒng)在電機(jī)工作的時(shí)候會(huì)產(chǎn)生大量的中高頻的震動(dòng)以及帶來大量風(fēng)，所以同時(shí)將IMU 模塊單獨(dú)使用一塊減震球放置，并且使用粘合劑使其在防抖消除震動(dòng)的效果上更加良好，同時(shí)方式氣壓計(jì)的數(shù)據(jù)被大風(fēng)干擾，還需要盡量將氣壓計(jì)與外界隔離，減少氣流對(duì)傳感器數(shù)據(jù)的影響。

1.3 系統(tǒng)電源設(shè)計(jì)

電源的輸入主要來自于12.4V 的航模3S 電池，經(jīng)過DC-DC 模塊，將電壓穩(wěn)定到5V 然后輸入到整個(gè)系統(tǒng)當(dāng)中，然后經(jīng)過防反接電路最后經(jīng)過不同的LDO 模塊，對(duì)3 塊MCU 的供電以及傳感器模塊供電。圖2 是電源設(shè)計(jì)的框架圖。

圖2 ：電源框架圖

2 系統(tǒng)軟件設(shè)計(jì)

2.1 ADRC控制器

ADRC 控制器設(shè)計(jì)：ADRC 控制算法又稱為自抗擾技術(shù)，該技術(shù)通過安排積分誤差產(chǎn)生過度過程，以及使用TD（跟蹤微分器）提取微分信號(hào)，以及使用非線性的誤差組合和采用ESO（擴(kuò)張狀態(tài)觀測(cè)器）估計(jì)總體擾動(dòng)。姿態(tài)環(huán)中使用四環(huán)串級(jí)的ADRC 控制。在高度環(huán)上使用PID 和ARDC 混合控制。作為本系統(tǒng)中的一個(gè)核心算法。

其中ADRC 控制器中，主要包含三大主要部分的結(jié)構(gòu)：

（1）跟蹤微分器；

（2）擴(kuò)張狀態(tài)觀測(cè)器；

（3）非線性反饋控制器。

跟蹤微分器將產(chǎn)生的誤差先經(jīng)過一個(gè)過渡過程，防止出現(xiàn)誤差的偏差過大的波動(dòng)。這樣能防止出現(xiàn)在傳統(tǒng)控制中出現(xiàn)的超調(diào)和響應(yīng)速度的矛盾。并且在一節(jié)慣性環(huán)節(jié)中，會(huì)不可避免的引入噪聲，通過跟蹤微分器的思想，將一階系統(tǒng)改為二階系統(tǒng)，這個(gè)二階系統(tǒng)就會(huì)對(duì)高頻噪聲產(chǎn)生一定的抑制作用。第二部分就是非線性反饋控制器。這個(gè)控制器是基于傳統(tǒng)控制器的線性疊加的想法改變而來，往往在實(shí)際工業(yè)場(chǎng)景中，線性組合的控制器并不能帶來比較好的控制效果，而非線性控制的輸入由跟蹤微分器TD 輸出的經(jīng)過過渡過程處理的誤差信號(hào)和信號(hào)的微分組成。并且同時(shí)將來自ESO 的三個(gè)狀態(tài)向量作為輸入所以總共由五項(xiàng)輸入。而ADRC 中的第三部分ESO 就是核心。其本質(zhì)是一個(gè)擴(kuò)張的狀態(tài)方程。通過觀測(cè)方程將輸入量和無關(guān)項(xiàng)擾動(dòng)全部觀測(cè)出來，通過擴(kuò)張狀態(tài)觀測(cè)器不僅可以得到各個(gè)狀態(tài)變量的估計(jì)，還能將四旋翼系統(tǒng)飛行的時(shí)候的不確定模型和外擾。以此作為反饋輸入到控制器中，對(duì)這種擾動(dòng)進(jìn)行補(bǔ)償。

圖3 為ADRC 的整體控制框圖。

圖3 ：ADRC 控制圖

2.2 飛行水平控制器

飛行器水平位置控制器:整體的飛行器系統(tǒng)的水平控制器由四個(gè)ADRC 環(huán)路構(gòu)成，具體為：三軸位置環(huán)+三軸速度環(huán)+三軸角度環(huán)+三軸角速度環(huán)的四級(jí)控制邏輯，并且通過光流，GPS，IMU 等模塊快的數(shù)據(jù)傳來，實(shí)現(xiàn)飛行器的水平位置不移動(dòng)以及飛行系統(tǒng)的姿態(tài)自穩(wěn)。要實(shí)現(xiàn)水平控制，設(shè)置達(dá)到懸停，四閉環(huán)控制是非常有必要，最里面兩環(huán)形成對(duì)姿態(tài)自穩(wěn)最基本的控制，而姿態(tài)控制又是四旋翼飛行器系統(tǒng)實(shí)現(xiàn)位置控制的基礎(chǔ)，外面兩環(huán)則是控制四旋翼三軸的速度及三軸位移的控制閉環(huán)，外面兩個(gè)環(huán)路對(duì)于實(shí)現(xiàn)四旋翼的穩(wěn)定選地有著非常重要的作用。通過使用光流得到一個(gè)相對(duì)的速度，加上加速度計(jì)的補(bǔ)償，得到飛行器的相對(duì)速度作為ADRC 速度環(huán)的反饋回路，而通過GPS 和加速度計(jì)兩次積分得到的位置數(shù)據(jù)得到XY 軸的位置速度，以此作為ADRC水平位置環(huán)的輸入。對(duì)位置的微分速度，而角度的微分則是角速度，同時(shí)三軸的速度期望作角度的輸入信號(hào)，在四旋翼運(yùn)動(dòng)不大時(shí)候，對(duì)于速度的期望相當(dāng)于對(duì)三軸角度的期望的，以此初步建立起四級(jí)串級(jí)閉環(huán)的ADRC 控制環(huán)路。實(shí)現(xiàn)通過圖4 為飛行器水平控制框圖。

圖4 ：飛行器水平控制框架圖

2.3 飛行定高控制器

飛行器高度控制器: 整體采用了三環(huán)串級(jí)PID 和ADRC控制，具體來說，是通過氣壓計(jì)、超聲波、GPS 以及加速度計(jì)的互補(bǔ)融合出估計(jì)的高度數(shù)據(jù)，以此作為反饋信號(hào)進(jìn)入系統(tǒng)的高度位置控制器，并且構(gòu)造位置環(huán)、油門Z 軸速度環(huán)、油門Z 加速度環(huán)的三環(huán)控制器。首席按接受來自接受機(jī)指令的高度值，作為期望值和經(jīng)過氣壓計(jì)、超聲波、GPS 三傳感器經(jīng)過數(shù)據(jù)融合產(chǎn)生的估計(jì)高度，兩者作差，產(chǎn)生誤差進(jìn)而輸入到高度位置的ADRC 控制環(huán)，該環(huán)的輸出信號(hào)作為下一環(huán)油門速度P 控制的信號(hào)，控制環(huán)的反饋信號(hào)是來自梯度下降法求解出來剔除Z 軸加速度的Z 軸速度，并且該Z 軸速度也是同樣作為一種油門的速度解釋而A 軸速度的控制使用的最簡(jiǎn)單的比例控制即P 控制器。該環(huán)的輸出繼續(xù)下一環(huán)Z 軸加速度的輸入，該控制環(huán)路使用的PID 控制器，因?yàn)閮?nèi)環(huán)控制的輸出最終是一個(gè)油門值，與PID 的傳統(tǒng)應(yīng)用非常類似，所以采用PID 控制。最后三環(huán)串級(jí)的定高控制系統(tǒng)設(shè)計(jì)完成。

圖5 為飛行器高度控制框圖。

圖5 ：飛行器高度控制框架圖

2.4 姿態(tài)解算系統(tǒng)

四旋翼飛行器在飛行過程中，受到氣體動(dòng)力學(xué)和電機(jī)升力的變化等因素影響，所以姿態(tài)解算是整飛行器系統(tǒng)的核心所在。本文的姿態(tài)解算主要是利用慣性測(cè)量元件IMU 中的加速度計(jì)、陀螺儀以及地磁計(jì)。使用IIC 或者SPI 協(xié)議先讀取傳感器的數(shù)據(jù)，即加速度計(jì)和陀螺儀的數(shù)據(jù)，濾波算法主要是將獲取到的陀螺儀和加速度計(jì)的數(shù)據(jù)融合進(jìn)行去噪聲及融合，得出正確的角度數(shù)據(jù)（歐拉角或四元數(shù)），數(shù)據(jù)融合常見的算法，主要采用互補(bǔ)濾波或者卡爾曼濾波。本實(shí)驗(yàn)使用的互補(bǔ)濾波的提升版，梯度下降的互補(bǔ)融合。其核心思想都是通過加速度計(jì)來補(bǔ)償陀螺儀的積分誤差。具體的實(shí)現(xiàn)過程是，先初始化四元數(shù)，通過DMA 宗信從負(fù)責(zé)姿態(tài)解算的MCU 中獲取傳感器的三軸加速度和角速度的數(shù)據(jù)，隨后加速度值規(guī)范化，用四元數(shù)求重力分量，計(jì)算重力分量和加速度測(cè)量的誤差，通過加速度計(jì)算出一套機(jī)體坐標(biāo)系下的四元數(shù)，再通過陀螺儀計(jì)算出另一組機(jī)體坐標(biāo)系下的四元數(shù)，然后構(gòu)造出目標(biāo)函數(shù)，求偏導(dǎo)，得到梯度下降融合公式，求得陀螺儀的誤差，并以此對(duì)陀螺儀得測(cè)量值進(jìn)行修正，將更新得到得新的陀螺儀下的四元數(shù)為新的四元數(shù)，然后反解出歐拉角即三軸姿態(tài)角。隨后算法不斷遞歸迭代，不斷更新四元數(shù)，從而求解出飛行器系統(tǒng)的姿態(tài)角。

2.5 實(shí)驗(yàn)結(jié)果與分析

本實(shí)驗(yàn)飛行器實(shí)現(xiàn)的懸停，穩(wěn)定起飛，跟蹤物體，以及識(shí)別相應(yīng)的物體降落等功能。借助于多串接的閉環(huán)控制算法，可以使得飛行器系統(tǒng)較平穩(wěn)低從地面靜止到在空中懸停的一整套動(dòng)作的完成。在進(jìn)入姿態(tài)控制模式的時(shí)候，遙控器的快速打桿后飛行器響應(yīng)速度快，跟隨性能好。同時(shí)在小角度剎車時(shí)候，飛行器的剎車效果比較可觀；在GPS 或光流定點(diǎn)懸停以及跟蹤物體時(shí)，定位精度在cm 級(jí)，在室外開闊場(chǎng)地能夠定位準(zhǔn)確。通過實(shí)際飛行和上位機(jī)的通信，可看出四旋翼的響應(yīng)以及抗干擾性的效果都比較不錯(cuò)。并且通過對(duì)四軸的簡(jiǎn)單建模，并且應(yīng)用ADRC 的控制算法進(jìn)行驗(yàn)證，使用matlab2016b 的Simulink 建模的仿真模型，通過整定參數(shù)，運(yùn)行仿真模型也可以進(jìn)一步看出，控制性能較好?？梢缘玫狡淇刂品抡鏀?shù)據(jù)波形圖6。綠色線的表示期望的輸入，而紅色線則是控制器的輸出，可以看出，ADRC 控制器的性能良好。

圖6 ：數(shù)據(jù)波形圖

3 結(jié)束語

本次設(shè)計(jì)，基于多MCU 的四旋翼飛行器從理論到系統(tǒng)硬軟件系統(tǒng)的實(shí)現(xiàn)，通過一步步實(shí)踐做出實(shí)物并驗(yàn)證了編程驗(yàn)證理論的合理性。在多MCU 系統(tǒng)中采用ADRC 控制算法的方面都有很不錯(cuò)的控制效果，本文提及的算法和軟件系統(tǒng)設(shè)計(jì)還需要進(jìn)一步優(yōu)化以及調(diào)試，以達(dá)到穩(wěn)定且可靠的效果。