小型四旋翼無人機(jī)姿態(tài)控制研究

徐琬婷，張強(qiáng)強(qiáng)，梅 源

（1.蕪湖職業(yè)技術(shù)學(xué)院 汽車與航空學(xué)院，安徽 蕪湖 241006；2.沈陽航空航天大學(xué) 人工智能學(xué)院，遼寧 沈陽 110136）

隨著嵌入式技術(shù)及計(jì)算機(jī)、傳感器技等相關(guān)技術(shù)的發(fā)展,小型四旋翼無人機(jī)以其結(jié)構(gòu)簡(jiǎn)單、易操作等優(yōu)勢(shì)在各個(gè)領(lǐng)域都得到了廣泛的應(yīng)用。小型四旋翼無人機(jī)由于自身承載能力有限，需要搭載體積小、慣性低和分辨率受限的傳感器，在極端環(huán)境下，容易受溫度、噪聲和震動(dòng)等因素影響，從而降低了無人機(jī)的姿態(tài)控制可靠性。為了解決這一問題，本設(shè)計(jì)利用了卡爾曼濾波有效增加了傳感器采集數(shù)據(jù)的準(zhǔn)確性，并采用了多種非線性控制方法，如PID 控制算法、滑模控制算法和LQR 控制算法等，有效增加了無人機(jī)的飛行性能、穩(wěn)定性、適應(yīng)性。

1 系統(tǒng)構(gòu)成及飛行姿態(tài)控制原理

1.1 系統(tǒng)總體架構(gòu)和功能概述

小型四旋翼飛行器的主要組成部分包括STM32103C8T6 主控芯片、OLED 顯示模塊、無線通信模塊、MPU6050 姿態(tài)信息采集模塊、光流模塊、電源模塊等。系統(tǒng)啟動(dòng)后，傳感器開始收集姿態(tài)和位置信息，并將數(shù)據(jù)傳輸回單片機(jī)進(jìn)行處理，實(shí)時(shí)通過液晶顯示屏顯示。本次測(cè)試所用的小型四旋翼無人機(jī)系統(tǒng)結(jié)構(gòu)如圖1 所示。

圖1 無人機(jī)系統(tǒng)結(jié)構(gòu)

小型四旋翼無人機(jī)的控制系統(tǒng)以STM32 為主控芯片，負(fù)責(zé)飛行動(dòng)作和數(shù)據(jù)處理。采用MPU6050 姿態(tài)傳感器獲取四旋翼飛行位置姿態(tài)信息，并利用GL9306 光流模塊作為位移傳感器感知無人機(jī)位置并實(shí)時(shí)發(fā)送給處理器進(jìn)行處理。SPL06 氣壓計(jì)模塊測(cè)得高度信息，并通過OLED 顯示屏實(shí)時(shí)展示位置姿態(tài)參數(shù)。HotRC 無線通信模塊實(shí)現(xiàn)與四旋翼無人機(jī)的通信和數(shù)據(jù)傳輸，以實(shí)現(xiàn)對(duì)無人機(jī)的即時(shí)控制。當(dāng)系統(tǒng)遭受強(qiáng)干擾、電源電壓低或飛行高度超過設(shè)定閾值范圍時(shí)，系統(tǒng)將緊急迫降以保障設(shè)備安全［1］。

1.2 四旋翼無人機(jī)的飛行原理

四旋翼無人機(jī)主要分為“X”形和“十”形，本次研究聚焦于“十”形結(jié)構(gòu)，當(dāng)槳葉高速旋轉(zhuǎn)時(shí)，螺旋槳產(chǎn)生向上的升力。當(dāng)電機(jī)達(dá)到一定轉(zhuǎn)速時(shí)，4 個(gè)螺旋槳所提供的升力大于其自身重力G，使無人機(jī)起飛。

為了確保無人機(jī)順利起飛，必須保證4 個(gè)槳葉具有相同的速度和角度，并產(chǎn)生相等的升力。螺旋槳旋轉(zhuǎn)方向示意圖見圖2，一對(duì)槳葉以相同速度旋轉(zhuǎn)方向相反的狀態(tài)旋轉(zhuǎn)，在靜態(tài)平衡下實(shí)現(xiàn)四旋翼本體抵消反扭矩效應(yīng)及陀螺效應(yīng)［2］。

圖2 四旋翼無人機(jī)的螺旋槳旋轉(zhuǎn)方向示意圖

四旋翼無人機(jī)可以通過兩組電機(jī)的協(xié)同作用實(shí)現(xiàn)多種姿態(tài)變化，包括懸停、翻滾、垂直運(yùn)動(dòng)、俯仰和偏航5 種運(yùn)動(dòng)狀態(tài)。而四旋翼無人機(jī)的姿態(tài)變化與4 個(gè)螺旋槳的轉(zhuǎn)向和轉(zhuǎn)速密切相關(guān)，只需精確控制各個(gè)旋翼的運(yùn)動(dòng)即可駕馭整體飛行。

1.3 懸停和垂直運(yùn)動(dòng)

四旋翼無人機(jī)應(yīng)在懸停的基礎(chǔ)上完成各種運(yùn)動(dòng)，因而懸停是最基本的運(yùn)動(dòng)狀態(tài)。當(dāng)螺旋槳產(chǎn)生的升 力F總（F總=F1+F2+F3+F4，其中F1、F2、F3、F4分別為機(jī)翼1、2、3、4 的升力）大于四旋翼無人機(jī)所受的重力時(shí)，將產(chǎn)生向上的加速度，使得無人機(jī)起飛；當(dāng)重力與升力相等時(shí)，實(shí)現(xiàn)了無人機(jī)的懸停狀態(tài)。在此基礎(chǔ)上，通過同時(shí)調(diào)整四個(gè)電機(jī)轉(zhuǎn)速可以實(shí)現(xiàn)垂直運(yùn)動(dòng)。四翼無人機(jī)受力分析如圖3 所示。若F總=重力G，則四旋翼無人機(jī)處于懸停狀態(tài)；若F總＜重力G，則四旋翼無人機(jī)會(huì)垂直下降。

圖3 四旋翼無人機(jī)受力分析

1.4 翻滾運(yùn)動(dòng)

設(shè)Front 指向四旋翼無人機(jī)的前方，當(dāng)四旋翼無人機(jī)處于懸停或前進(jìn)狀態(tài)時(shí)執(zhí)行順時(shí)針或逆時(shí)針轉(zhuǎn)操作可使四翼無人機(jī)進(jìn)行滾動(dòng)運(yùn)動(dòng)。在保持垂直方向總升力不變的情況下，通過調(diào)整1 號(hào)和3 號(hào)電機(jī)的轉(zhuǎn)速來實(shí)現(xiàn)順時(shí)針或逆時(shí)針翻滾。同時(shí)，控制當(dāng)前姿態(tài)變化的2 號(hào)和4 號(hào)旋翼增加轉(zhuǎn)速以保持四旋翼無人機(jī)高度穩(wěn)定。當(dāng)F1＞F2=F4＞F3時(shí)，四旋翼無人機(jī)將進(jìn)行逆時(shí)針滾動(dòng)；而當(dāng)F3＞F2=F4＞F1時(shí)，則會(huì)進(jìn)行順時(shí)針滾動(dòng)，如圖4 所示。

1.5 俯仰運(yùn)動(dòng)

當(dāng)需要將無人機(jī)控制到特定位置時(shí)，僅通過翻滾是不足以完成任務(wù)的，還需結(jié)合俯仰運(yùn)動(dòng)。俯仰運(yùn)動(dòng)與翻滾運(yùn)動(dòng)的控制原理相同，通過保持2 號(hào)和4號(hào)電機(jī)轉(zhuǎn)速不變，并調(diào)節(jié)1 號(hào)和3 號(hào)電機(jī)轉(zhuǎn)速來實(shí)現(xiàn)前后移動(dòng)。然而，在非控制當(dāng)前姿態(tài)變化的旋翼1號(hào)和3 號(hào)轉(zhuǎn)速保持不變時(shí)四旋翼高度不變。當(dāng)F2＞F1=F3＞F4時(shí)，四旋翼無人機(jī)前傾；當(dāng)F4＞F1=F3＞F2時(shí)，四旋翼無人機(jī)后仰。

1.6 偏航運(yùn)動(dòng)

除了具備上下、前后直線運(yùn)動(dòng)功能外，四旋翼無人機(jī)還能實(shí)現(xiàn)偏航運(yùn)動(dòng)，偏航運(yùn)動(dòng)通過同時(shí)控制兩組旋翼的轉(zhuǎn)速來實(shí)現(xiàn)。其中2 號(hào)和4 號(hào)電機(jī)為一組，1 號(hào)和3 號(hào)電機(jī)為一組，在不同轉(zhuǎn)速下利用反扭力差異來達(dá)到偏航運(yùn)動(dòng)的目的。當(dāng)F2=F4＞F1=F3時(shí)，四旋翼無人機(jī)逆時(shí)針偏航；當(dāng)F2=F4＜F1=F3時(shí)，四旋翼無人機(jī)順時(shí)針偏航。

2 軟件數(shù)據(jù)處理

小型四旋翼飛行器在飛行過程中需要實(shí)時(shí)處理大量準(zhǔn)確的數(shù)據(jù)，否則經(jīng)過多次計(jì)算后會(huì)產(chǎn)生較大誤差，從而對(duì)四旋翼無人機(jī)的姿態(tài)判斷和飛行性能造成影響。為了減少環(huán)境因素和干擾信號(hào)對(duì)飛行的影響，在搭載傳感器輸出數(shù)據(jù)時(shí)需要進(jìn)行濾波處理，并結(jié)合控制算法以提高小型四旋翼無人機(jī)的精確性和抗干擾能力［3］。

2.1 數(shù)據(jù)處理

MPU6050 姿態(tài)傳感器可直接輸出6 軸信息，直接輸出的6 軸數(shù)據(jù)精度可達(dá)到0.01°，經(jīng)過對(duì)6 軸信息進(jìn)行姿態(tài)解算可以得到四旋翼無人機(jī)的姿態(tài)角。定義繞X 軸旋轉(zhuǎn)為俯仰角θ,繞Y 軸旋轉(zhuǎn)為滾轉(zhuǎn)角?,繞Z 軸旋轉(zhuǎn)為偏航角ψ。

2.1.1 DMP 解算方式

MPU6050 是一款高性能的六軸慣性測(cè)量單元，內(nèi)部集成了運(yùn)動(dòng)處理單元DMP（Digital Motion Processor），通過DMP，用戶可以直接從MPU6050 中讀取四元數(shù)（q1，q2，q3，q4），并將導(dǎo)航坐標(biāo)系轉(zhuǎn)換為機(jī)體坐標(biāo)系，其轉(zhuǎn)換關(guān)系可表示為

其中：

且四元數(shù)滿足條件

q1，q2，q3，q4分別為

（xb,yb,zb）為機(jī)體坐標(biāo)，（xn,yn,zn）為導(dǎo)航坐標(biāo)。

由式（1）可得，四旋翼無人機(jī)的姿態(tài)角為θ、?、ψ,即

在解算四旋翼無人機(jī)飛行姿態(tài)角時(shí)，可用DMP硬件進(jìn)行四元數(shù)計(jì)算，并利用式（2）中的轉(zhuǎn)換關(guān)系將其轉(zhuǎn)化為姿態(tài)角。在使用硬件DMP 進(jìn)行姿態(tài)解算時(shí)，仍然存在漂移現(xiàn)象（包括溫度漂移和零點(diǎn)漂移）、自身振動(dòng)對(duì)姿態(tài)角產(chǎn)生抖動(dòng)影響和傳感器受到具有不確定性的環(huán)境因素（如磁場(chǎng)變化等）的干擾，因此，需要采用數(shù)據(jù)處理算法對(duì)數(shù)據(jù)進(jìn)行濾波處理以使環(huán)境變量的影響最小化。

2.1.2 卡爾曼濾波方式

卡爾曼濾波是一種用于估計(jì)系統(tǒng)狀態(tài)的數(shù)學(xué)方法，它根據(jù)當(dāng)前時(shí)刻的觀測(cè)值和前一個(gè)時(shí)刻的預(yù)測(cè)值之間的誤差，通過計(jì)算系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)，并預(yù)測(cè)下一個(gè)時(shí)刻的狀態(tài)［4］。

MPU6050 解算過程可分為3 步。首先，從MPU6050 讀取3 軸加速度計(jì)和3 軸陀螺儀的數(shù)據(jù)。其次，通過積分處理3 軸加速度計(jì)值，并通過幾何運(yùn)算將3 軸陀螺儀數(shù)據(jù)轉(zhuǎn)換為相應(yīng)的姿態(tài)角。由于電場(chǎng)、磁場(chǎng)等因素的干擾，所得到的姿態(tài)數(shù)據(jù)容易受到影響，因此，需引入卡爾曼濾波算法對(duì)數(shù)據(jù)進(jìn)行融合濾波處理，以提高姿態(tài)數(shù)據(jù)的準(zhǔn)確性。圖5 為DMP姿態(tài)解算方式和卡爾曼濾波姿態(tài)解算方式的MATLAB 仿真結(jié)果。

圖5 DMP 姿態(tài)解算方式和卡爾曼濾波姿態(tài)解算方式MATLAB 仿真結(jié)果

由圖5 可知，用DMP 方式解算姿態(tài)角時(shí)存在大量干擾信號(hào)，輸出的姿態(tài)角波動(dòng)比較大，而使用卡爾曼濾波濾除了大部分的干擾信號(hào)，輸出的姿態(tài)角更加接近真實(shí)值，大大地改善了四旋翼無人機(jī)控制系統(tǒng)的控制性能。

2.2 控制算法分析

為了更好地驗(yàn)證經(jīng)過濾波后的效果，系統(tǒng)分別采用串級(jí)PID 控制算法、滑模控制算法和LQR 控制算法等對(duì)無人機(jī)姿態(tài)進(jìn)行控制，以選擇最好的方法提高小型無人機(jī)的可靠性［5］。

2.2.1 串級(jí)PID 控制算法

串級(jí)PID 控制算法包含2 個(gè)控制環(huán)，外環(huán)用于位置控制，內(nèi)環(huán)用于姿態(tài)控制。以位置環(huán)的輸出作為姿態(tài)環(huán)的輸入，為保證四旋翼無人機(jī)運(yùn)行穩(wěn)定性，對(duì)位置和姿態(tài)環(huán)輸出進(jìn)行限幅處理。串級(jí)PID 控制器原理如圖6 所示。

圖6 串級(jí)PID 控制器控制原理圖

根據(jù)圖6，輸入位置信息為（x，y，z）=（5，5，5），經(jīng)過系統(tǒng)反饋后，得到位置信號(hào)偏差，并在位置環(huán)中應(yīng)用PID 算法進(jìn)行處理以獲得期望姿態(tài)角度θ、?和ψ，然后將姿態(tài)角度傳入姿態(tài)環(huán)中進(jìn)行PID運(yùn)算，從而生成PWM 輸出并驅(qū)動(dòng)四旋翼無人機(jī)飛行至指定位置，仿真結(jié)果如圖7 所示。

圖7 四旋翼無人機(jī)串級(jí)PID 控制仿真結(jié)果

由圖7 知，采用串級(jí)PID 控制的四旋翼無人機(jī)表現(xiàn)出較快的響應(yīng)速度，在約3 s 內(nèi)即能達(dá)到穩(wěn)定狀態(tài)。此外，從滾轉(zhuǎn)角和俯仰角的變化曲線也可以觀察到初始階段存在較大波動(dòng)，而偏航角始終保持不變，則說明在姿態(tài)調(diào)節(jié)過程中無人機(jī)并未受電機(jī)力矩變化等因素影響而發(fā)生旋轉(zhuǎn)等對(duì)穩(wěn)定性產(chǎn)生負(fù)面影響。超調(diào)量達(dá)到了16%，證明采用串級(jí)PID 控制四旋翼無人機(jī)具備良好的控制效果。

2.2.2 滑模控制算法

滑模控制器仍采用雙閉環(huán)結(jié)構(gòu)，外環(huán)為位置環(huán)，其輸出作為內(nèi)環(huán)姿態(tài)環(huán)的期望輸入，并由系統(tǒng)當(dāng)前狀態(tài)實(shí)時(shí)調(diào)節(jié)系統(tǒng)的運(yùn)行速度。滑模控制算法分析仿真結(jié)果如圖8 所示。由圖8 知，在零初始狀態(tài)下給定輸入位置信號(hào)為（x，y，z）=（5，5，5），小型四旋翼的響應(yīng)得到顯著改善。系統(tǒng)反應(yīng)迅速，X、Y、Z 軸在不到2 s 的時(shí)間內(nèi)即可達(dá)到指定位置，并且超調(diào)量?jī)H為4%。但滾轉(zhuǎn)角和俯仰角出現(xiàn)波動(dòng)，導(dǎo)致機(jī)身抖動(dòng)，降低了四旋翼系統(tǒng)的穩(wěn)定性和抗干擾性能。

圖8 滑模控制算法仿真結(jié)果

2.2.3 LQR 控制算法

LQR（Linear Quadratic Regulator）是一種線性二次型調(diào)節(jié)器［6］，LQR 基于全狀態(tài)反饋原理，通過加權(quán)求和不同狀態(tài)得到最優(yōu)控制量。作為一種最優(yōu)控制策略，LQR 屬于全狀態(tài)反饋控制結(jié)構(gòu)，在系統(tǒng)運(yùn)行中以最小代價(jià)實(shí)現(xiàn)系統(tǒng)操作，LQR 控制算法的結(jié)構(gòu)原理如圖9 所示。

運(yùn)行LQR 控制算法時(shí)，需要對(duì)系統(tǒng)狀態(tài)準(zhǔn)確估計(jì)，并對(duì)其進(jìn)行離散化處理。在離散情況下，可以從遞歸和迭代的角度思考，并通過多次迭代達(dá)到設(shè)定的穩(wěn)定值。本研究采用LQR 控制方法對(duì)四旋翼飛行器進(jìn)行仿真，仿真結(jié)果如圖10 所示。

由仿真結(jié)果知，當(dāng)輸入位置信息（x，y，z）=（5，5，5）時(shí)，四旋翼無人機(jī)展現(xiàn)出了迅速的響應(yīng)能力，起飛初期在X 軸和Y 軸方向的飛行速度最高可達(dá)到3 m/s。在4 s 內(nèi)，四旋翼無人機(jī)已完成調(diào)節(jié)并進(jìn)入穩(wěn)態(tài)狀態(tài)，并且其在Z 軸方向上輸出值已達(dá)到穩(wěn)態(tài)值的90%。在第5 s 時(shí)，四旋翼無人機(jī)成功實(shí)現(xiàn)設(shè)定位置并完全進(jìn)入穩(wěn)態(tài)穩(wěn)定飛行階段。相較于之前2 種算法而言，無人機(jī)到達(dá)穩(wěn)態(tài)的時(shí)間比較長(zhǎng)。

3 結(jié)論

本文主要研究了基于STM32 單片機(jī)的小型四旋翼無人機(jī)的姿態(tài)控制問題，并對(duì)四旋翼無人機(jī)性能組態(tài)調(diào)整的快速響應(yīng)進(jìn)行了仿真測(cè)試。仿真結(jié)果表明，姿態(tài)信息經(jīng)過卡爾曼濾波后，能夠有效降低誤差和外部干擾信號(hào)對(duì)數(shù)據(jù)的影響，顯著提升飛行器所收集數(shù)據(jù)的準(zhǔn)確性和可靠性。

所用到姿態(tài)控制的3 種算法各有優(yōu)缺點(diǎn)，滑模控制算法具有快速響應(yīng)特性和較小超調(diào)量，在系統(tǒng)出現(xiàn)擾動(dòng)時(shí)可以迅速調(diào)整。LQR 控制算法對(duì)系統(tǒng)模型的精確性要求較高，穩(wěn)定性良好，但動(dòng)態(tài)性能相對(duì)較差，串級(jí)PID控制算法的反應(yīng)速度不及滑模控制算法迅捷，并且存在一定超調(diào)量，具備更佳的穩(wěn)態(tài)特性和抗干擾能力。