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

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

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

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

中圖分類號：TP391.9 ? ? 文獻標識碼：A

1 ? 引言（Introduction）

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

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

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

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

2.1 ? 豎直上升和豎直下降

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

2.2 ? 前向運動和后向運動

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

2.3 ? 左向運動和右向運動

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

2.4 ? 順時針變向和逆時針變向

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5 ? 結(jié)論（Conclusion）

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

參考文獻（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] 歷小偉，郭玉英.四旋翼飛行器的動力學建模與飛行控制[J].自動化與儀器儀表，2017（1）：130-132.

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

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

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

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

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

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

作者簡介：

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

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

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

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

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