馬鑫源 李燕燕 梁雅雯 張勇 陳琦

摘 ?要：直接對(duì)四旋翼飛行器控制系統(tǒng)進(jìn)行調(diào)試，極易引發(fā)危險(xiǎn)，嚴(yán)重時(shí)還會(huì)造成人員傷害，針對(duì)目前多采用數(shù)值仿真驗(yàn)證方法難以直接展示控制效果問題，本文設(shè)計(jì)一種基于Matlab/SimMechanics軟件的四旋翼飛行器飛行控制可視化仿真系統(tǒng)，首先，基于Simulink完成了四旋翼飛行器控制系統(tǒng)設(shè)計(jì)，然后采用PID控制算法實(shí)現(xiàn)了四旋翼飛行器飛行狀態(tài)直觀姿態(tài)和飛行數(shù)據(jù)分析功能，最后通過仿真可以得出結(jié)論，所設(shè)計(jì)的四旋翼飛行器控制可視化仿真系統(tǒng)通過三維可視化窗口，可實(shí)時(shí)完成四旋翼飛行器實(shí)時(shí)控制算法演示和仿真，具有重要的應(yīng)用價(jià)值。

關(guān)鍵詞：四旋翼飛行器;SimMechanics;可視化仿真系統(tǒng)

中圖分類號(hào)：TP391.9 ? ? 文獻(xiàn)標(biāo)識(shí)碼：A

1 ? 引言（Introduction）

四旋翼飛行器是一個(gè)典型的不穩(wěn)定、時(shí)變、強(qiáng)耦合、高度非線性的欠驅(qū)動(dòng)系統(tǒng)。其由四個(gè)旋翼的推力驅(qū)動(dòng)，卻具有六個(gè)運(yùn)動(dòng)自由度，六個(gè)自由度之間又是相互耦合的。四旋翼飛行器的這些特點(diǎn)給飛行控制器設(shè)計(jì)帶來了較大的困難。國(guó)外一些研究者已經(jīng)使用三維模型研究如六足機(jī)器人等復(fù)雜機(jī)器人結(jié)構(gòu)[1];Fred[2]、Gaifullin[3]等已開始對(duì)四旋翼飛行器實(shí)行三維建模仿真分析。國(guó)內(nèi)有研究者如歷小偉，郭玉英[4]采用試湊法與理論推導(dǎo)法結(jié)合進(jìn)行調(diào)參/仿真;楊則允、李猛孫、欽鵬[5]通過搭建simulink模型進(jìn)行數(shù)值仿真。若只采用Matlab/Simulink對(duì)控制進(jìn)行設(shè)計(jì)及數(shù)值仿真驗(yàn)證，無法將四旋翼飛行器的六個(gè)自由度的數(shù)據(jù)進(jìn)行整合，直觀的給研究者展現(xiàn)四旋翼飛行器的實(shí)時(shí)位置及其姿態(tài)狀況，因此需要搭建一個(gè)三維可視化仿真環(huán)境，通過參比實(shí)物飛行器搭建出飛行器的三維實(shí)體模型，在實(shí)物飛行器試飛前對(duì)控制系統(tǒng)進(jìn)行可視化仿真，以驗(yàn)證控制系統(tǒng)是否符合設(shè)計(jì)要求。SimMechanical是是一種基于Simulink的研究與分析仿真環(huán)境平臺(tái)，其可為動(dòng)力機(jī)械系統(tǒng)及其控制系統(tǒng)提供簡(jiǎn)明有效地建模分析手段，所有工作都在該環(huán)境中完成。它給予了許多與實(shí)際系統(tǒng)相關(guān)的部件，如：剛體、鉸鏈、坐標(biāo)系、執(zhí)行機(jī)構(gòu)、傳感器等。利用這些模塊，即可以方便地構(gòu)建復(fù)雜機(jī)械系統(tǒng)的模型，又能對(duì)機(jī)械系統(tǒng)或Simulink設(shè)計(jì)的任何控制器進(jìn)行單獨(dú)分析，并與其他動(dòng)態(tài)系統(tǒng)一起實(shí)現(xiàn)綜合仿真[6]。

本文設(shè)計(jì)了一種基于Matlab/SimMechanics的四旋翼飛行器可視化仿真控制系統(tǒng)。首先通過三維建模建立現(xiàn)有四旋翼飛行器的三維實(shí)體模型，隨后在Simulink中構(gòu)建四旋翼飛行器控制器模塊，并與搭建好的四旋翼飛行器模型相連接，即可對(duì)四旋翼飛行器的控制算法進(jìn)行可視化仿真分析。在所設(shè)計(jì)的可視化仿真系統(tǒng)實(shí)驗(yàn)環(huán)境下，可以完成四旋翼飛行器進(jìn)行飛行測(cè)試，模擬實(shí)際飛行狀態(tài)，并對(duì)控制器參數(shù)進(jìn)行合理調(diào)試和可靠性驗(yàn)證，評(píng)估四旋翼飛機(jī)實(shí)際飛行狀態(tài)，其大大提高了飛行控制器設(shè)計(jì)的效率，提前發(fā)現(xiàn)實(shí)際飛行測(cè)試控制器設(shè)計(jì)中的不合理，來減少不必要的損失，確保實(shí)際飛行測(cè)試的安全。

2 ?四旋翼飛行器的基本工作原理（Basic working principle of quadrotor）

四旋翼飛行器的工作原理主要通過控制分布在四個(gè)角落的電機(jī)旋轉(zhuǎn)力度的大小和方向，驅(qū)動(dòng)螺旋槳葉片旋轉(zhuǎn)產(chǎn)生升力，從而達(dá)到控制飛行器的姿態(tài)和位置的目的。四旋翼飛行器能夠在空中完成八種不同的運(yùn)動(dòng)，分別是豎直上升、豎直降落、前向飛行、后向飛行、左向飛行、右向飛行、順時(shí)針變向、逆時(shí)針變向。下面詳細(xì)介紹各個(gè)動(dòng)作的工作原理。

2.1 ? 豎直上升和豎直下降

四旋翼飛行器飛行至空中穩(wěn)定后，四個(gè)電機(jī)M1、M2、M3、M4轉(zhuǎn)速同時(shí)增大或同時(shí)減小，此時(shí)可以進(jìn)行豎直上升或豎直降落運(yùn)動(dòng)，圖1為豎直上升運(yùn)動(dòng)原理圖，圖2所示為豎直降落原理圖。

2.2 ? 前向運(yùn)動(dòng)和后向運(yùn)動(dòng)

四旋翼飛行器飛行至空中穩(wěn)定后，電機(jī)M2、M3轉(zhuǎn)速增大，電機(jī)M1、M4轉(zhuǎn)速不變或者減小，可完成前向飛行，圖3為前向飛行原理圖。電機(jī)M2、M3轉(zhuǎn)速減小或不變，電機(jī)M1、M4轉(zhuǎn)速增加，可完成后向飛行，圖4為后向飛行原理圖。

2.3 ? 左向運(yùn)動(dòng)和右向運(yùn)動(dòng)

四旋翼飛行器飛行至空中穩(wěn)定后，電機(jī)M1、M2轉(zhuǎn)速增大，電機(jī)M3、M4轉(zhuǎn)速不變或減小，可完成左向飛行，圖5為左向飛行原理圖。電機(jī)M1、M2轉(zhuǎn)速減小或不變，電機(jī)M3、M4轉(zhuǎn)速增加，可完成右向飛行，圖6為右向飛行原理圖。

2.4 ? 順時(shí)針變向和逆時(shí)針變向

四旋翼飛行器飛行至空中穩(wěn)定后，電機(jī)M1、M3轉(zhuǎn)速增大，電機(jī)M2、M4轉(zhuǎn)速不變或減小，可完成順時(shí)針變向飛行，圖7為順時(shí)針變向飛行原理圖。電機(jī)M1、M3轉(zhuǎn)速減小或不變，電機(jī)M2、M4轉(zhuǎn)速增加，可完成逆時(shí)針變向飛行，圖8為逆時(shí)針變向飛行原理圖。

3 ?四旋翼飛行器算法（Explanation of quadrotor algorithm）

3.1 ? 姿態(tài)解算簡(jiǎn)介

姿態(tài)解算是飛行器飛行的關(guān)鍵技術(shù)之一，它指控制器讀取自身傳感器數(shù)據(jù)，實(shí)時(shí)計(jì)算四旋翼飛行器的姿態(tài)角，例如橫滾角、俯仰角、偏航角的信息，控制器根據(jù)這些信息即可計(jì)算四個(gè)電機(jī)的輸出量，使飛行器保持平衡穩(wěn)定或者保持某一傾斜角往設(shè)定方向飛行。姿態(tài)解算速度和精度直接關(guān)系到飛行器飛行中的穩(wěn)定性和可靠性[7]。關(guān)于姿態(tài)解算有很多方法，擴(kuò)展型卡爾曼濾波和互補(bǔ)濾波其中兩種經(jīng)典的方法[8]。其中互補(bǔ)濾波算法對(duì)系統(tǒng)要求較低，計(jì)算量小，而擴(kuò)展型卡爾曼濾波算法精度高，但計(jì)算量大，且需要建立精確的動(dòng)力學(xué)模型，因此在四旋翼飛行器的姿態(tài)計(jì)算中常應(yīng)用互補(bǔ)濾波算法。

3.2 ? 軟件姿態(tài)解算

歐拉角描述一次平面旋轉(zhuǎn)（坐標(biāo)變換），如圖9所示。

坐標(biāo)系繞旋轉(zhuǎn)α角后得到坐標(biāo)系，在空間中有一個(gè)矢量在坐標(biāo)系中的投影為，在內(nèi)的投影為由于旋轉(zhuǎn)繞進(jìn)行，Z坐標(biāo)不變，即有：

上式中左側(cè)是更新后的歐拉角，對(duì)應(yīng)roll、pitch、yaw。右側(cè)是上個(gè)周期測(cè)算出來的角度，三個(gè)角速度為四旋翼飛行器計(jì)算間隔為T陀螺角速度。求解此微分方程可解算出當(dāng)前的歐拉角。

4 ?四旋翼飛行器控制系統(tǒng)設(shè)計(jì)及可視化仿真（Design and visual simulation of four-rotor aircraft control system）

4.1 ? 搭建四旋翼飛行器可視化仿真模型

在Matlab軟件里的SimMechanics附件中提供了大量可建立運(yùn)動(dòng)機(jī)構(gòu)模型模塊，這些模塊可以通過Simulink連接設(shè)計(jì)完成控制器，從而完成綜合系統(tǒng)仿真。SimMechanics在三維建模方面存在一定的局限性，為了彌補(bǔ)這方面的不足，本文依據(jù)現(xiàn)有的四旋翼飛行器的形狀和尺寸，如圖10所示，采用三維繪圖軟件完成四旋翼飛行器的三維實(shí)體建模，將其置于SimMechanics中，并且與設(shè)計(jì)好的飛行器控制器進(jìn)行連接。四旋翼飛行器主體由一個(gè)機(jī)體和四個(gè)加裝螺旋槳的電機(jī)構(gòu)成，機(jī)體與電機(jī)間通過單自由度模塊連接構(gòu)成一個(gè)完整的四旋翼飛行器，四旋翼飛行器三維實(shí)體模型如圖11所示。

SimMechanics內(nèi)部提供了驅(qū)動(dòng)和傳感器模塊，搭建的仿真模型可以便捷地與Simulink中的其他模塊進(jìn)行數(shù)據(jù)傳輸，可在同一條件下構(gòu)建控制器和受控對(duì)象模型進(jìn)行仿真模擬。四旋翼飛行器具有六個(gè)空間自由度，因此在地坐標(biāo)系與四旋翼飛行器模型之間必須具有六自由度模塊，并且必須擁有角度、角速度及位置傳感器[9]。為使四旋翼飛行器模型產(chǎn)生升力，需要對(duì)模型中四個(gè)單自由度模塊添加驅(qū)動(dòng)力矩模塊和由力矩產(chǎn)生的轉(zhuǎn)速傳感器模塊，將其傳遞給對(duì)應(yīng)螺旋槳，帶動(dòng)螺旋槳轉(zhuǎn)動(dòng)。最后調(diào)整模塊里的重力方向與仿真圖中的Z軸方向保持一致，最終完成四旋翼無人機(jī)在SimMechanics中模型，如圖12所示。

4.2 ? 四旋翼飛行器控制系統(tǒng)設(shè)計(jì)

PID控制算法實(shí)際上就是對(duì)偏差e（t）進(jìn)行比例、積分、微分變換的運(yùn)算[10]，具備結(jié)構(gòu)簡(jiǎn)單、成本低、操控方便、穩(wěn)、準(zhǔn)、快等優(yōu)點(diǎn)，在工業(yè)控制中應(yīng)用十分廣泛。本文采用串級(jí)PID控制算法作為四旋翼飛行器控制系統(tǒng)的主要算法，來控制四旋翼飛行器的飛行姿態(tài)，串級(jí)PID控制器結(jié)構(gòu)圖如圖13所示。

在串級(jí)PID控制器中，內(nèi)環(huán)選用PID控制器控制飛行器姿態(tài)角，simulink程序如圖14所示，外環(huán)采用PD控制器控制飛行器位置，simulink程序如圖15所示。

圖16為在Simulink中搭建的四旋翼飛行器控制系統(tǒng)的仿真模型。系統(tǒng)輸入量即為目標(biāo)預(yù)期位置X_des，Y_des偏航角accX_fdfrwd、accY_fdfrwd、accZ_fdfrwd，由位置控制器計(jì)算出期望的滾轉(zhuǎn)角yaw_des，俯仰角roll_des，pitch_des，由姿態(tài)控制器和運(yùn)動(dòng)學(xué)逆解算法，四旋翼飛行器每個(gè)電機(jī)所需轉(zhuǎn)矩控制量由期望姿態(tài)角和高度所計(jì)算出，將轉(zhuǎn)矩控制量導(dǎo)入四旋翼飛行器三維模型，并在Simulink模塊里構(gòu)建其控制器模塊，可在可視化仿真系統(tǒng)里對(duì)其進(jìn)行校驗(yàn)。

4.3 ? 四旋翼飛行器控制系統(tǒng)可視化仿真

為驗(yàn)證四旋翼飛行器控制系統(tǒng)性能，本文對(duì)所設(shè)計(jì)的控制器進(jìn)行模擬仿真，初始狀態(tài)四旋翼飛行器的默認(rèn)位置為：X_des=0，Y_des=0，Altitude=0;期望的輸入軌跡為：X_des=0.5*sin（t），Y_des=0.5*cos（t），Altitude=1，輸出的四旋翼飛行器的實(shí)際軌跡為X、Y、Alt，圖17即為四旋翼無人機(jī)的可視化動(dòng)態(tài)仿真圖，其中圖18（a）為四旋翼飛行器的側(cè)視效果圖，圖18（b）則為四旋翼飛行器的俯視效果圖。X軸方向的軌跡追蹤曲線如圖18所示，從圖中可觀察到從起飛至飛行平穩(wěn)穩(wěn)后，系統(tǒng)基本可以追蹤上X軸的輸入。高度的追蹤曲線如圖19所示，由圖可觀察到在1s左右的時(shí)候高度存在一個(gè)超調(diào)量，4s左右恢復(fù)到目標(biāo)位置，跟蹤誤差基本為零。

圖20為四旋翼飛行器在期望輸入下的仿真飛行軌跡，軌跡圖形符合預(yù)期。整個(gè)模擬過程在仿真系統(tǒng)里清晰簡(jiǎn)潔，效果真實(shí)，數(shù)據(jù)準(zhǔn)確?；诖怂男盹w行器模型，也能方便的對(duì)控制器的參數(shù)進(jìn)行整定。一是通過對(duì)simulink控制器中的PID模塊直接手動(dòng)進(jìn)行調(diào)節(jié)參數(shù)調(diào)節(jié);二是可以通過PID模塊中提供的Tune方法，讓系統(tǒng)對(duì)系統(tǒng)被控對(duì)象進(jìn)行辨識(shí)，通過設(shè)定期望的系統(tǒng)響應(yīng)性能，simulink通過后臺(tái)模擬仿真給出相應(yīng)PID的PID參數(shù)建議值，同時(shí)，在simulink中能輕松直觀的對(duì)不同參數(shù)下系統(tǒng)的響應(yīng)進(jìn)行比較分析，直觀地看到不同參數(shù)下飛行器的3D飛行效果，可減少調(diào)節(jié)PID參數(shù)的盲目性，加快控制器參數(shù)整定速度。如圖21為系統(tǒng)在定高飛行實(shí)驗(yàn)中，不同的P值下四旋翼飛行器的高度變化曲線。

5 ? 結(jié)論（Conclusion）

本文經(jīng)過導(dǎo)入三維模型，快速搭建實(shí)現(xiàn)了四旋翼飛行器的SimMechanics可視化仿真模型，并通過與Simulink模塊所實(shí)現(xiàn)的PID控制器相結(jié)合，構(gòu)造了一個(gè)四旋翼飛行器控制仿真系統(tǒng)。所構(gòu)建的四旋翼飛行器控制仿真系統(tǒng)可以實(shí)現(xiàn)實(shí)體無人機(jī)控制算法的仿真及分析等功能，并能夠直觀地觀察到四旋翼飛行器的即時(shí)飛行狀態(tài)，便捷靈活，效果逼真。四旋翼飛行器控制仿真系統(tǒng)能夠在開發(fā)過程中加深對(duì)四旋翼飛行器控制算法的仿真研究，降低開發(fā)成本。

參考文獻(xiàn)（References）

[1] LudovicoMinati，MattiaFrasca，Natsue Yoshimura.Versatile Locomotion Control of a Hexapod Robot Using a Hierarchical Network of Nonlinear Oscillator Circuits[J].IEEE Access，2018，6（99）：8042-8065.

[2] Fred Delcomyn，Mark E Nelson.Architectures for a biomimetic hexapod robot[J].Robotics & Autonomous Systems，2000，30（2）：5-15.

[3] A.M.Gaifullin，O.V.Animitsa，I.S.Bosnyakov.Modeling of Aircraft Flight Through the Wake Vortex[J].Journal of Applied Mechanics and Technical Physics，2019，60（2）：314-322.

[4] 歷小偉，郭玉英.四旋翼飛行器的動(dòng)力學(xué)建模與飛行控制[J].自動(dòng)化與儀器儀表，2017（1）：130-132.

[5] 楊則允，李猛，孫欽鵬.四旋翼無人機(jī)控制系統(tǒng)仿真設(shè)計(jì)[J].計(jì)算機(jī)測(cè)量與控制，2019，27（4）：68-71.

[6] 陳云.基于SimMechanics和VRML的6-dof并聯(lián)振動(dòng)平臺(tái)的運(yùn)動(dòng)仿真[D].吉林大學(xué)，2012：66-73.

[7] 蔡明成.基于STM32的四軸飛行器控制系統(tǒng)的研究與設(shè)計(jì)[D].安徽理工大學(xué)，2017：44-52.

[8] 張棟，焦嵩鳴，劉延泉.互補(bǔ)濾波和卡爾曼濾波的融合姿態(tài)解算方法[J].傳感器與微系統(tǒng)，2017，36（3）：62-66.

[9] 許江陰，趙宏強(qiáng)，鄧宇.四旋翼無人機(jī)可視化軌跡跟蹤仿真系統(tǒng)[J].計(jì)算機(jī)測(cè)量與控制，2017（03）：138-141.

[10] 徐巍.基于姿態(tài)控制的四旋翼飛控系統(tǒng)研究[D].南昌航空大學(xué)，2016：74-80.

作者簡(jiǎn)介：

馬鑫源（1998-），男，本科生.研究領(lǐng)域：虛擬仿真.

李燕燕（1998-），女，本科生.研究領(lǐng)域：控制算法.

梁雅雯（1996-），女，本科生.研究領(lǐng)域：系統(tǒng)仿真.

張 ?勇（1978-），男，博士，副教授.研究領(lǐng)域：智能檢測(cè)與信息處理.本文通訊作者.

陳 ?琦（1971-），女，博士，副教授.研究領(lǐng)域：智能決策與信息處理.